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ABSTRACT 
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This  report  develops  10  different  approaches  to  the  target  tracking 
problem  based  on  bearing  only  observations.  Its  purpose  is  to  form  the 
basis  for  an  extensive  simulation  study,  aimed  at  achieving  the  best 
possible  tracking  performance  for  this  tracking  problem.  Included  in 
the  report  are  also  a  proposed  initialization  routine  for  bearing 
only  trackers,  and  a  maneuver  detection  algorithm  with  a  proposed  action 

scheme  following  the  maneuver  detection. 
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SUMMARY  AND  CONCLUSIONS 


Ten  different  approximative  nonlinear  filtering  approaches  to  the 
target  tracking  problem  based  on  bearing  only  measurements  have  been 
developed. 

These  approaches  include  two  Extended  Kalman  filters  (Cartesian  and 
Polar  coordinate  system  representation) ,  two  second  order  Gaussian  rilters 
(Cartesian  and  Polar),  f^va  different  iterated  Extended  Kalman  filters, 
and  one  approach  consisting  of  M  parallel ,  Extended  (Cartesian)  Kalman 
filters . 

The  polar  coordinate  system  representation  of  the  target  motion 
provided  the  necessary  insight  and  equations  for  us  to  attack  the 
initialization  problem  based  on  bearing  only  information.  Based  on  th» 
two  assumptions:  1)  Straight  tarqet  trajectory  and  2)  Noisefree  bearings, 
we  could  show  that  given  a  target  range,  the  target  velocity  components 
could  be  calculated  from  three  consecutive  bearings,  with  no  constraints 
on  the  observer's  trajectory  over  the  observation  interval.  We  could  also 
show,  that  if  the  observer's  velocity  was  zero,  we  could  calculate  the 
target's  heading  exact,  even  if  the  assumed  range  was  false. 

By  including  a  fourth  bearing,  and  introducing  the  constraint  that 
the  observer  should  be  maneuvering  during  the  observation  interval,  we 
also  derived  the  equation  for  calculation  of  range  to  target. 

In  order  to  take  into  account  the  fact  that  the  observations  are  noisy, 
a  procedure  for  smoothing  of  initial  range  and  velocity  data  calculated 
from  a  batch  of  bearing  observations  was  proposed. 

In  order  to  adapt  the  straight  trajectory  filters  to  the  curved 
trajectory  case,  which  results  from  a  maneuvering  target,  we  derived  the 


IV 


structure  for  a  likelihood  ratio  test  based  on  the  principle  of  comparing 
the  innovation  sequence  with  its  expected  statistics,  based  on  the  "no 
maneuvre"  hypothesis.  When  the  actual  variance  of  the  innovation  sequence 
becomes  consistently  larger  than  its  expected  variance  over  the  most 
recent  L  samples,  the  "target  maneuvre"  hypothesis  is  accepted. 

The  proposed  action  scheme  following  the  maneuvre  detection  included 
two  main  features: 

1.  A  reprocessing  of  the  observations  ontained  in  the  time 
period  At  prior  to  the  maneuver  detection,  where  At  is 
the  nominal  time  delay  between  the  start  of  the  maneuvre 
and  its  detection. 

2.  Imposing  limited  memory  on  the  filter,  by  increasing  the 
velocity  elements  of  the  covariance  matrix,  before  the 
reprocessing  of  observations  described  above  takes  place. 

The  result  of  this  scheme  will  be  a  better  utilization  of  the  obser¬ 
vations  obtained  during  the  target  maneuvre.  When  the  reprocessing  of 
the  observations  obtained  during  the  time  period  At  are  finished,  a  dis¬ 
crete  jump  to  a  more  correct  position  and  velocity  of  the  state  vector 
at  the  time  when  the  maneuvre  was  detected,  will  be  the  result. 

The  purpose  of  this  report  is  to  be  a  mathematical  basis  for  an 
extensive  simulation  study  where  the  performance  of  the  different  filtering 
approaches  to  bearing  only  tracking  can  be  compared. 

When  this  simulation  study  has  been  performed,  we  will  be  in  the 
position  to  give  an  answer  to  the  question:  To  what  extent  is  it  pos¬ 
sible  to  overcome  the  three  main  problems  with  bearing  only  tracking: 

1)  The  poor  degree  of  observability,  2)  The  nonlinearity  of  the  problem, 
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claiming  for  the  best  possible  linearization  trajectory,  and  3)  The 
target  maneuvre  detection  and  handling  problem. 

We  have  not  made  any  effort  to  analyze  and  compare  the  computational 
complexity  nor  the  memory  capacity  requirements  of  the  different  filters. 
The  reason  for  this  is  that  we  didn't  want  these  aspects  of  the  problem 
to  constraint  our  choice  of  filtering  approaches  at  this  stage. 
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1.1  Problem  Statement 

Our  objective  is  to  investigate  the  target  tracking  problem  based 
on  bearing  observations  from  a  single,  moving  observer. 

Our  approach  will  be  threefold: 

1.  Derive  different  mathematical  approaches  to  the  filtering 
problem  for  targets  moving  along  straight  trajectories, 

2.  Derive  more  optimal  initialization  routines  based  only  on 
bearing  observations  from  the  moving  observer. 

3.  Derive  maneuvre  detection  algorithms,  which,  together 
with  specific  action  patterns  following  the  maneuvre 
detection,  allows  the  straight  trajectory  filters  to 
adapt  tc  curved  trajectories. 

We  restrict  the  problem  to  tracking  in  the  x-y-plane. 

The  observer's  position,  velocity  and  acceleration  are  known 
functions  of  time,  with  known  accuracy  (lo  values). 

The  bearing  sensor's  accuracy  is  known  (la). 
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1.2  Report  Outline 

The  composition  of  this  report  can  be  described  as  follows: 

Chapter  2  introduces  the  difficulties  that  exists  for  this  type  of 
tracking,  and  gives  an  outline  of  the  different  approaches  to  straight 
trajectory  tracking  that  will  be  developed. 

In  Chapter  3  we  develop  the  mathematical  equations  for  10  different 
filtering  approaches  to  this  tracking  problem. 

In  Chapter  4  the  initialization  problem  is  analyzed,  and  necessary 
equations  for  an  initialization  routine  for  bearing  only  trackers  are 
proposed . 
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Chapter  5  gives  a  short  resume  over  earlier  approaches  to  maneuvering 
target  tracking,  and  continues  with  the  detailed  equations  for  a  proposed 
maneuvre  detection  and  handling  scheme  for  the  different  filtering 
approaches  given  in  Chapter  3. 

The  purpose  of  this  report  is  to  form  the  basis  for  a  simulation 
study,  where  the  different  tracking  approaches  are  compared ,  and  the  fil¬ 
tering  approach  wi  -.h  the  best  overall  performance  can  be  selected. 

Chapter  6  gives  a  few  guidelines  to  be  taken  into  account  while  planning 
and  performing  this  simulation  study. 
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2.  PROBLEM  ANALYSIS  AND  APPROACH  PROPOSALS 

The  main  problem  with  this  type  of  tracking  is  the  poor  degree  of 
observability.  It  is  well  known  that  the  success  of  this  tracking 
scheme  depends  entirely  on  the  observer's  maneuvering.  Simultaneously, 
in  most  cases,  the  tracker  has  to  be  based  on  a  maneuvering  scheme 
selected  on  other  criterias  than  tracking  performance.  This  is  due  to 
reasons  lrke: 

-  Narrow  waters 

-  Not  reveal  the  observer's  position 

-  Not  restrict  the  captain's  decision  space. 

The  topic  of  selecting  the  optimal  maneuvre  in  order  to  maximize 
the  observability  has,  however,  been  treated  by  D.J.  Murphy  [1] . 

Another  problem  is  the  necessity  to  perform  linearization  about 
the  target  trajectory.  However,  this  trajectory  is  unknown.  The  pur¬ 
pose  of  our  tracker  is  to  estimate  this  trajectory.  Now,  if  lineari¬ 
zation  is  performed  about  the  a  priori  estimate  of  the  state  vector, 
which  is  the  most  obvious  thing  to  do,  the  utilization  of  observations, 
when  the  estimated  trajectory  is  far  from  the  correct  one,  will  be 
poor.  This  is  the  case  in  the  initialization  phase,  and  after  a 
target  maneuvre. 

A  proposed  initialization  routine  for  bearing  only  trackers  will 
be  developed  in  Chapter  4. 

The  third  main  problem  with  this  type  of  tracking  is  the  maneuvre 
detection  and  handling  problem.  If  the  target  performs  a  maneuvre 
after  a  stable  target  solution  is  established,  it  is  possible  to  detect 
the  occurrence  of  the  maneuvre  (after  a  certain  period  of  time)  without 
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the  observer  maneuvering.  However,  in  order  to  arrive  at  the  new, 
post-tnaneuvre  target  course  and  speed,  an  observer  maneuvre  normally 
has  to  take  place.  This  is  due  to  the  time  delay  between  the  maneuvre 
occurrence  and  its  detection,  resulting  in  range  to  target  error  at 
the  target  maneuvre  detection  time  point.  Since  this  time  delay  will 
vary,  depending  on  the  type  and  size  of  the  target  maneuvre,  the  range/ 
velocity  ambigu^.  v  cannot  be  resolved  exactly  without  an  observer 
maneuvre.  Maneuvre  detection  and  following  actions  patterns  will  be 
proposed  in  Chapter  5. 

The  question  is  now:  How  can  these  three  problems  be  overcome? 

Or  to  put  it  more  correctly:  To  what  extent  do  different  approaches 
to  this  target  tracking  scheme  overcome  these  three  problems? 

Existing  literature  on  the  subject  fail  to  give  an  answer  to  this 
question. 

Our  intention  is,  therefore,  to  develop  a  number  of  different 
approaches  to  this  target  tracking  problem,  which  can  form  the  basis 
for  an  extensive  simulation  study,  where  the  different  approaches  are 
compared.  When  this  simulation  study  is  finished,  our  basis  for  giving 
a  reliable  answer  to  the  question  above  should  be  greatly  improved. 

The  following  10  approaches  cure  proposed: 

1.  Extended  Kalman  filter,  Cartesian  Coordinate  system  repre¬ 
sentation.  State  vector: 

r  x  i 


y 


(2.1) 
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observation  equation: 

+  w  (2.2) 

where 

x  =  target's  x  -  coordinate 
y  =  target's  y  -  coordinate 
v^  =  target's  x  -  velocity 

=  target’s  y  -  velocity 

x  =  observer's  x  -  coordinate 
s 

y  =  observer's  Y  -  coordinate 
cj)  =  bearing  from  observer  to  target  rel.  north, 
w  =  observation  noise 

2.  Extended  Kalman  filter,  polar  coordinate  system  representation. 
State  vector: 


Observation  equation: 


<j>  =  [1  0  0  Olx^  +  w 


(2.4) 
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where  4>  r  v^, 


v  and  w 

y 


are  defined  earlier,  and 


R  =  range  from  observer  to  target 


3.  Second  order  Gaussian  filter,  representation  as 

4.  Second  order  Gaussian  filter,  representation  as 

5.  iterated  Extended  Kalman  filter,  representation 
iteration  scherau  ;  in  Jazwinski  [2]  ,  pp.  278-279. 


in  1. 
in  2. 
as  in 


It 


6.  Iterated  Linear  Filter-Smoother.  Representation  as  in  2, 
iteration  scheme  as  in  Jazwinski  [2] ,  pp.  279-281. 


7.  Global  Iterated  Filter,  Representation  as  in  1,  iteration 
scheme  as  proposed  by  Jazwinski  [2],  pp.  281. 

8.  Global  Iterated  Filter.  Representation  as  in  2,  iteration 
scheme  as  in  7. 


9.  Extended  Kalman  filter,  Cartesian  coordinate  system  repre¬ 
sentation.  State  vector: 


LV»J 


(2.5) 


where 


N 

c 

3c 


=  May  be  variable  number  >  1 

=  State  vector  at  sample  k,  representation  as  in  1. 


3c-n 


=  State  vector  at  sample  k-N,  representation  as  in  1. 


Observation  equation: 


(2.6) 


This  scheme  will  process  each  observation  twice.  Details  outlined  in 
Chapter  3.6.3  ("Serial"  filter  approach) 


10.  Parallel  filter  approach.M  filters,  each  with  Cartesian  coordinate 
system  representation  as  in  approach  1,  are  initialized  with  range 


R±  =  Rq  +  i  •  Ar,  i  =  1,2,...,M  (2.7) 

where  RQ  and  AR  are  constants.  The  a  priori  variances  on  R^  for  each 
filter  are  assumed  low,  in  order  to  stablize  range  for  each  filter. 

The  resulting  state  vector  for  this  filtering  scheme  is  given  by: 


^(R^R/z^. 


(2.8) 


where  the  probability  p(Ri  =  R/z^)  has  to  be  determined 
Chapter  3.7. 


Details  in 


3.1  Extended  Kalman  filter,  Cartesian  Coordinate  System. 

The  Cartesian  Coordinate  system  representation  is  the  most  common 
way  to  model  the  target  motion  dynamics.  Several  papers  on  target 
tracking  use  this  representation  [3] -[11],  and  most  probably  in  the 
majority  of  the  existing  target  tracking  systems  of  this  kind  in  operating 
order,  this  representation  is  used. 

Assuming  the  target  is  nonmaneuvering ,  the  following  mathematical 
model  can  be  established; 


The  geometrical  situation  is  depicted  in  Fig.  3.1; 
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X 


X 


s 


Pig.  3.7.  Target-Observer-Geometry. 

The  different  variables  in  equations  (3.1)  and  (3.2)  should  be 

T 

self-explaining  from  Fig.  3.1,  except  for  w  and  v  =  [v^  v^l  .  These 
are  the  measurement  and  the  process- noise,  respectively. 

Since  the  tracker  is  going  to  be  realized  on  a  digital  computer ,  the 
discrete  version  of  the  equations  (3.1)  and  (3.2)  are  sought.  We  get 


(1/T  -  sample  frequency)  [3] : 

J^+1  =  <J>(T)xk  +  9(T)v  k 

(3.3) 

^  ■  9(4>  +  \ 

(3.4) 

Here,  $(T) ,  6(T)  and  gtx^)  are  defined  by: 
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v  0 

lk 


0  v. 


(3.9) 


Further,  we  define: 


=  -ia_ 


-k,k-l  (_  k 


R2  '  R2 
k  k 


0  0 


(3.10) 


where  ^  ^  is  the  a  priori  estimate  of  the  state  vector  at  time  k. 

•The  Extended  Kalman  filter  equations  for  the  system  described  by 
equations  (3.3)  to  (3.10)  are  the  following: 


Initialization : 


-0,-1  -0 


(3.11) 


p  =  to 

*0,-1  *0 


(3.12) 


Observation  integration: 


%,k  ^k,k-l  +  Kk(zk  ~  *k,k-l) 


(3.13) 


Sk,k-1  g(^c,k-l) 


(3.14) 


K*'pk,k-1<<Vk,k-iHk+V'1 


(3.15) 


pk,k  -  (I'W\k-i(I-WI+'\^ 


(3.16) 


Time  updating: 


=  *IT)i,k 


PJt+l,k  =  *'T)Pk,k(*,(T’T  +  9(WVk8(T)T 


(3.17) 


(3.18) 


A  discrete  Polar  coordinate  system  representation  of  the  non- 
maneuvering  target  can  be  derived  mathematically,  however,  the  easiest 
way  is  to  use  a  geometrical  approach.  Fig.  3.2  shows  the  relative 
geometrical  situation,  as  seen  from  the  observer  (the  coordinate 
system  is  fixed  to  the  observer) : 


By  refering  to  Fig.  3.2,  the  following  two  equations  can  be  estab¬ 
lished  directly: 


\vvi 


*k+l  =  +  tan 


+  V 


lk 


(3.19) 


\+i  +ALNk  +  v; 


2k 


(3.20) 
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where  and  v2k  are  additional  white  Gaussian  noise  processes. 

The  variables  Al^  and  Al^  are  the  relative  displacement  between 
observer  and  target  over  the  sample  period  [kT,  (k+l)T],  along  and 
across  the  line  of  sight  to  target,  c^.  These  variables  can  be  given 
by: 


•  « 

/ 

r  n 

•  m 

%k 

... 

.V 

AyTk 

4*Sk 

where  the  transformation  matrix,  s  is  given  by: 


(3.21) 


Sk" 


cos  4>k 

-sin 

sin  <|>k 

cos 

(3.22) 


T  T 

and  [Ax^Ay^]  and  [Ax^k  Ay^]  are  the  target’s  and  the  observer's 
absolute  displacement  in  x,y  -  direction  during  the  sample  period 
[kT,  (k+l)T] . 

Since  the  assumption  is  made  that  the  target  is  not  maneuvering,,  we 
have: 


A*Tk' 

vxk 

=  T  . 

'  t> 

l _ _ 

-VYK 

(3.23) 


where 


v  =  target  absolute  velocity  in 


v^  =  target  absolute  velocity  in 


x-direction  at 
y-direction  at 


time  kT 
time  kT 
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The  observer's  displacement  during  the  sample  period  [kT,  (k+l)T] , 

T 

[Ax^Ay  ]  ,  is  given  by  the  observer's  dead  reckoning  system.  Assuming, 
that  the  observer's  acceleration  is  constant  over  the  sample  period,  we 
can  write: 


vsxk 

.  T  +  ^  T2* 

asxk 

.Aysk. 

V  ’k. 

2 

_asyk_ 

where 


vsxk  =  ^server's  velocity  in  the  x-direction  at  time  kT 
v  ^  =  observer's  velocity  in  the  y-direction  at  time  kT 


a  ,  =  observer's  acceleration  in  the  x-direction  at  time  kT 
syk 

ag^k  =  observer's  acceleration  in  the  y-direction  at  time  kT 


Now,  selecting  the  Cartesian  velocity  components  v^  and  v  ^  as 
representation  for  the  target  velocity,  the  total,  nonlinear  Polar 
coordinate  system  representation  of  the  target-observer  relationship, 
is  given  by: 


\+l 

* +  tm  (w) 

vlk 

= 

^W5  + 

+ 

V2k 

vx  k+l 

Vxk 

V3k 

vy  k+l 

,VYk 

_V4k- 

(3.25) 
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equation  (3.25)  can  be  written: 

+  2* 

Further,  defining  the  observation  matrix: 
H  =  [1  0  0  0] 


(3.26) 


(3.27) 


(3.28) 


(3.29) 


the  observation  equation  for  the  system  described  by  equation  (3.28) 
will  be: 

Zk  =  H  ’  i  +  wk  (3.30) 

Equations  (3.28)  and  (3.30)  are  our  final  equations  for  the  Polar 
coordinate  system  version  of  the  target  tracking  problem.  As  we  can 
see,  the  system  dynamics  are  nonlinear,  while  the  observation  equation 


is  linear. 
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In  order  to  utilize  the  Extended  Kalman  filter  on  this  system,  we 
have  to  develop: 


'4  U  =  £l 


The  development  of  is  done  in  APPENDIX  A.  The  result  is: 


where 


V\  *  4V 

Nk 

T.AL  , 
yk 

IiLkk 

Ci 

4i 

Ci 

Ci 

VS* 

vv 

T-iLxk 

Fk  = 

Vi 

Vi 

Vi 

Vi 

0 

0 

i 

0 

. 

0 

0 

0 

1 

Al  . 
xk 

■  «k +  “V  3in  *k +  %k 

•  cos  <j>k 

Al 

yh 

“  <sk +  003  *k  -  %k 

sin  <J>k 

or,  by  inserting  for  Al^  and  Al^  from  equation  (3.21) 
=  \  •  sin  *k  +  Nk  •  ^sk 


“yk  *  \  °°s  *k  +  iyTk  -  iy: 


(3.31) 


(3.32) 


(3.33) 


(3.34) 


sk 


(3.35) 

(3.36) 


The  Extended  Kalman  filter-equations  for  the  system  described  by 
equations  (3.28)  and  (3.30)  are  the  following: 
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Initialization : 


°W? 

II 

rH 

1 

(3.37) 

pP  =  pP 

0,-1  0 

(3.38) 

Observation  Integration : 

^k,k  =  3c,k-l  +  Kk(zk”£k,k-1> 

(3.39) 

^  /sD 

Zk,k-1  =  H%,k-1 

(3.40) 

\  -  ’k.k-ZK.k-/ +  v'1 

(3.41) 

*1U =  (I-vK,k-1(i-v)T  +  w£ 

(3.42) 

Time  updating: 

4i.k  *  i<k> 

(3.43) 

^0 

X 

it 

£ 

** 

+ 

X% 

(3.44) 

where  the  process  noise  covariance  matrix,  is  given  by 


*- 


4 


4 


o 

0 


0  vP 

3k 


0 

0 

0 


0  vL 

4k 


(3.45) 


In  order  to  get  identical  initial  conditions  for  the  two  repre¬ 


sentations  given  in  Section  3.1  and  3.2,  the  following  relationship 
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exists  between  the  initial  conditions  given  in  equations  (3.11),  and 
(3.37),  (3.38): 


sin  4>q  0 

cos  4>q  0 

0  1 

0  0 


Further,  defining: 


T  = 


“ 

R  cos  <j>Q 
-R  sin  4>q 


0 

0 


sin  0  0 

cos  4>q  0  0 

0  10 

0  0  1 


we  have: 


(3.46) 


(3.47) 


(3.48) 


3.3  Second  Order  Gaussian  Filters. 

The  following  development  follows  the  spirit  of  Jazwinski  [2] , 
pp.  91,  336-346,  362-365,  and  Gelb  [12],  pp.  191-192.  We  are,  however, 
interested  in  a  system  representation  where  both  system  dynamics  and 
observation  are  discrete,  while  [2]  and  [12]  are  concerned  with  continuous 
system  representation,  and  discrete  observations.  Therefore,  the  discrete 
version  of  the  second  order  time  updating  equations  have  to  be  developed. 

Jazwinski  [2]  defines  4  different  possible  solutions  to  second  order 


filtering: 
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1.  The  truncated  second  order  filter. 

2.  The  Gaussian  second  order  filter. 

3.  The  modified  truncated  second  order  filter. 

4.  The  modified  Gaussian  second  order  filter. 


approach  no.  4,  which  also  has  been  developed  by  Wishner 
For  this  approach  the  following  assumptions  are  made 

is  symmetric  and  "close  to  the  mean". 

2.  The  third  central  moment  is  zero. 

3.  Assuming  Gaussian  density,  the  fourth  central  moment  is 
approximated  by: 

E  'ij-iv  <v4>  +  pjltpi*  +  pklpij  (3-49) 

4.  The  fifth  and  higher  order  moments  are  neglected. 


We  select 
et  al.  [13]. 
about  p (x^) ; 

i.  p<v 


In  order  to  proceed  the  development,  a  couple  of  operators  have  to 
be  defined,  namely  1)  3  2 (£(x)  ,B)  and  2)  (32_f  P232f) : 

2 

1)  The  operator  9x(.f(x)  ,B) ,  for  any  function  £(x) ,  any  x  and 
any  matrix  B  is  a  vector  whose  i*"*1  element  is  defined  by: 


32 . (f ,B)  =  trace 
xi  — 


3x 


3f . 
— i 


3x 


B 


(3.50) 


2  2  2 

2)  The  operator  [S^-f  (x)  P  3^f (x) ] ,  for  any  function  f(x),  any  x  and 


any  symmetric  matrix  P,  is  a  symmetric  matrix  with  elements  (i, j)  [2] : 
n 


32f . 

i 


32f . 


Pn  P, 


.  3x.  3x„  £p  kq  3x  3x 

k,  ,p,q  Tc  l  *  p  ( 


(3.51) 
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Wishner  et  al  [13]  gives  another  definition  of  this  operator,  which 
turn  out  to  give  identical  result.  The  [i,j]  element  is  in  [13]  given 
by: 

““»js|  4  (|i(5,,]T  ■  4<f3<£,) T  •  PS  (3'52> 

In  the  following,  we  will  use  the  definition  in  eq-  (3.52)  for  this 
operator. 

With  the  definition  of  these  two  operators  in  mind,  we  are  ready  to 
develop  the  modified  Gaussian  second  order  filter. 

The  system  and  observation  equations  are  assumed  to  have  the  following 
form: 
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We  recall  the  definitions  of  equations  (3.10)  and  (3.31).  Now,  taking 
the  expectations  of  equations  (3.55)  and  (3.56),  making  use  of  the 
assumptions  that: 


-  4,k}  “  2 

(3.57) 

E{v4,k-1)  "  2 


we  get: 


4+i, k =  E{£<4}  “  £(4,k> + 1  \  pk,k> 

4k-i  ■  E{3<4}  -  9<4,k-i> +  i  \ 


(3.58) 


(3.59) 


Equation  (3.58)  is  the  second  order  approximation  difference  equation 
for  between  observations.  We  now  seek  a  recursive  equation  for  the 
covariance  matrix  P  between  observations .  Using  (3.53),  (3.55),  (3.31) 

JC 

and  (3.58)  we  get: 


Pk+l,k  (^k+l"-k+l,k)  -k+l"\+l,k)  * 


E{[Fk*A^k,k  +  2  ^  k-'  A-k,k  *  A^k,k)  2  k(-,Pk,k)  +  ^k1*1*1  * 


=  FP  F  +  V  +  L. 
k  k,k  k  k  Tc 


(3.60) 


where  L  is  given  by: 
k 


4  ‘  7  E{84,k(i'  i4,k^,k>-84fk<5'  44,k44,k)T) 

t  < 

k,k 


- 1  tk<£4.k>i  v«'»k.k»* 


(3.61) 
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By  use  of  the  approximation  given  in  equation  (3.49) ,  can  be  reduced 
to  [2]  : 


\  - 1 4  i 

k,k 


(k.  jp:  .  a: 

-k,k  k,k  x^  ^ 


f(4,i 


>) 


(3.62) 


Equations  (3.60)  and  (3.62)  are  the  sought  recursion  for  between 
observations. 

The  equations  for  the  modified  Gaussian  second  order  filter  at  an 
observation  is  given  in  [2]  : 


A 


^k 


»k 


+  WVk-i1 


(3.63) 


pk,k  -  “-WVk-i^-W1  +  W* 


(3.64) 


\  ^k-lWk^-A  +  wk  +  V 


(3.65) 


2(9Xk^k_1?(^k,k-l)Pk,k-l3^>k_1  g(4,k-l): 


(3.66) 


The  necessary  equations  for  the  modified  Gaussian  second  order  filter 
are  now  established,  and  constitutes  of  equations  (3.58),  (3.60)  and 
(3.62)  for  time  updating,  and  (3.63),  (3.59),  (3.64)  -  (3.66)  for  ob¬ 
servation  integration. 

The  equations  will  nov;  be  specialized  to  the  two  different  representa¬ 
tions  of  the  target  tracking  problem. 


3.3.1  Cartesian  Coordinate  System  Model 

The  Cartesian  system  model  representation  of  the  target  motion  with 
bearing  only  measurements  is  given  by  equations  (3.3)  to  (3.7).  Since 
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the  system  dynamics  are  linear,  the  second  order  filter  equations  for  time 
updating,  equations  (3.58),  (3.60)  and  (3.62)  reduces  to  the  normal  Kalman 
filter  equations,  given  by  equations  (3.17)  and  (3.18).  The  observation 
equation  (3.4),  however,  is  nonlinear,  and  equations  (3.59)  and  (3.66) 
have  to  be  evaluated  for  the  nonlinear  function  g (x^)  given  by  equation 
(3.7). 

From  equations  (3.59)  and  (3.7)  we  get: 


,  /  x  -x  ,  \ 

S  _  .—-1/  k.k-1  sk  \ 

^yk.k-rysJ 


+  b. 


(3.67) 


where  b^  is  given  by  (see  equations  (3.59)  and  (3.50)): 


(3.68) 


1  ^2  ..  -1,  *  ,  1  .  (  3  r  3  tan  1  ( - )! T i  „  I 

=  1  4,k-l  '  =  2  trace(¥  L~9i - j  I  ' 

-“%,k-l 


Defining  (see  Fig.  3.1) 


Ax  = 


\,k-l  Xsk 


-  yk,k-l  ‘  ysk 


■Ak2  +  iv2 


(3.69) 


we  can  calculate 


3x 


~  .  -1  ,Ax. 

3  tan 


3x 
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The  result: 


T 2 Ax -Ay 


_3_ 

-l(- 
3  tan  X\A 

9x 

3x 

A  2  A  2 

Ax  -Ay 


0 

0 


A  2  A  2 

Ax  -Ay 


2 Ax- Ay 
R4 

0 

0 


0 

0 


0 

0 


(3.70) 


Based  on  equation  (3.70),  equation  (3.68)  becomes 


bk=^ 
K  2  R 


trace 


-2AxAy  •  P11  +  (Ax2-Ay2)P21»  -2AxAyP12  -s-  (Ax2-Ay2)P22  0  0 

(Ax2-Ay2)Pi;L  +  2AxAy*P21,  (Ax2-Ay2)F12  +  2AxAyP22  0  0 


0 

0 


0 

0 


Finally: 


“k  ■  7T4  I^AylP^-P^)  +  tt-W)  (P21  +  P12) 


2  R 


0  0 

0  0 
(3.71) 

<3.72) 


Next  we  have  to  evaluate  equation  (3.66)  based  on  equations  (3.52)  and 
(3.7).  The  result  is 


\  =  ~1  t2Ax2Ay2(P21+P22-P12-P2J)  -  2AxAy(Ax2-Ay2)(Pn-P22)(P12+P2l) 

JR 

+  (Ax  -Ay  )  (p11P22  +  Pi2*P21^ 


(3.73) 


In  equation  (3.71)  -  (3.73)  the  variables  P^,  P12,  P21,  and  P22  are 


the  position  elements  of  the  covariance  matrix  P  ,  i.e., 

Jv  f 
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11 

P12 

P13 

P14 

21 

P22 

P23 

P24 

31 

P32 

P33 

P34 

» 

41 

P42 

P43 

P44 

(3.74) 


The  modified  Gaussian  second  order  filter  for  the  Cartesian  system 
model  representation  of  the  target  motion  can  then  be  summarized  as  follows: 


Initialization :  Equations  (3.11)  and  (3.12). 

Observation  Integration :  Equations  (3.13),  (3.67),  (3.72),  (3.65),  (3.73) 
and  (3.16). 

Time  updating:  Equations  (3.17)  and  (3.18). 


3.3.2  Polar  Coordinate  System  Model 

The  polar  coordinate  system  representation  of  our  cracking  problem  is 
given  by  equations  (3.28)  and  (3.30).  In  this  case  the  system  dynamics 
are  nonlinear,  while  the  observation  equation  is  linear. 

The  second  order  filter  equations  for  time  updating,  equations  (3.58), 
(3.60)  and  (3.62)  have  then  to  be  specialized  to  our  polar  coordinate  system 
model,  while  the  observation  equations,  equations  (3.63)  —  (3. 66 ) ,  (3.59), 
reduces  to  the  normal  Extended  Kalman-filter  equations,  given  by  equations 
(3.39)-(3.42) . 


From  equation  (3.58)  we  get: 


✓vP  r-  ,^vP  .  ,  1  ~ 

*k+l,k  =  —  ^k,k  +  2 


(3.75) 
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where  the  vector  has  elements  i  given  by: 


C.  .  =  trace  ,  ^ 
ki  \  3x 


{3.76) 


Further,  we  have  to  calculate  the  matrix  given  by  equation  (3.62). 

The  calculation  of  and  are  performed  in  APPENDIX  B.  The  results 

are: 


Z  fl..P.. 
3i  ID 


V 

C2k 

C3k 

.C4k. 

Z  f2..P.. 
<-i  31  13 


0 

0 


(3.77) 


\ml 


4  4 

Z  s..s..  Z  s..t.. 


j  ,i=l  13 


•  .  ,  31  13 
3,1=1  J  J 


2  t , . s . .  I  t..t.. 

,  ]i  i] 

3,1=1  J  J  3,1=1  J  J 


0 

L  0 


0 

0 


0 

0 


(3.78) 


The  elements  of  the  matrices  FI,  F2,  S  and  T  are  given  in  APPENDIX  B. 

The  modified  Gaussian  second  order  filter  for  the  Polar  Coordinate 
system  model  of  the  tracking  problem  can  then  be  summarized  as  follows-: 


Initialization;  Equations  (3.37)  and  (3.38) 


Observation  integration:  Equations  (3 . 39) - (3.42) , 

Time  updating;  Equations  (3.75),  (3.77),  (3.60)  and  (3,78). 

3.4  iterated  Extended  Kalman  Filter  [2] 

The  following  5  approaches  to  solve  the  target  tracking  problem  are 
all  approaches  involving  some  sort  of  iteration  scheme.  The  first  approach, 
called  the  iterated  extended  Kalman-filter,  is  a  local  iteration  scheme, 
and  is  designed  in  order  to  reduce  the  effect  of  measurement  function 
nonlinearity.  This  approach  can  therefore  be  tried  on  the  Cartesian 
coordinate  system  representation,  which  have  linear  system  dynamics  and 
nonlinear  measurement  model. 

Since  this  approach  is  very  well  documented  in  Jazwinski  (2] ,  pp.  278- 
279,  it  should  be  unnecessary  to  repeat  the  equations  in  this  text.  For 
completeness ,  however,  the  approach  is  include;''  in  APPENDIX  C. 

3.5  Iterated  Linear  Filter-Smoother  [2] 

If  we  have  system  dynamics  nonlinearities,  the  preceding  iterator  will 
not  improve  the  estimate  x^+1  k  due  to  system  nonlinearities  acting  on  the 
interval  [kT,  (k+l)T]. 

The  preceding  iterator  can  therefore  not  be  used  in  connection  with 
the  polar  coordinate  system  representation  of  the  target  motion. 
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The  "Iterated  Linear  Filter  Smoother",  proposed  by  Jazwinski  [2], 
pp.  279-281,  include  the  time  updating  process  in  the  iteration  loop. 

This  approach  can  therefore  be  tried  on  the  polar  coordinate  system  repre¬ 
sentation  of  our  tracking  problem. 

Again,  since  the  approach  is  well  documented  in  [2] ,  the  iterator 
equations  are  exiled  to  APPENDIX  D. 

3.6  Global  Iterated  Filters 

As  we  mentioned  in  Section  2,  one  of  the  main  problems  with  bearing 
only  tracking  is  the  poor  degree  of  observability.  The  observability  of 
range  is  entirely  dependent  on  the  maneuvering  scheme  of  the  observer. 

In  order  to  improve  the  observability,  and  at  the  same  time  improve 
the  reference  trajectory  and  thereby  get  a  more  optimal  utilization  of 
each  observation,  a  number  of  global  iteration  schemes  are  proposed. 

The  iterations  will  here  not  be  performed  at  the  time  kT  (as  in  section 
3.4)  or  over  the  time  interval  [kT,  (k+l)T]  (as  in  section  3.5) ,  but  over 
the  greater  time  interval  [(k-N)T,  kT] ,  where  N  is  some  integer  constant, 
may  be  variable. 

This  iterator  requires,  however,  a  lot  of  memory  capacity  for  storing 
of  variables.  The  following  sequences  has  to  be  stored  (M  is  an  integer 
constant  >  N) : 

Observation  Sequence: 


\  =  {zk-M'  Vm+1'  Zk-M+2'*-*'zk} 


(3.79) 
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Observer  position,  velocity  and  acceleration: 


Xsk  =  {^s,  — 


=  •  ,;-sk 

k-M  k-M+i 


V  k  "  <v  ,  v  f-fvj 
k  k-M  k-M+1  k 


A  k  =  {a  ,  a  /••./§,  .} 

K  k-M  k-M+1  * 


covariance  matrix: 


Pk  *Pk-M,k-M""'Pk,k* 


(3.80) 


(3.82) 


(3.83) 


As  we  can  see,  the  memory  capacity  requirements  will  depend  on  the 
size  of  the  integer  M.  Also,  if  the  iteration  time  interval  is  fixed 
(N  =  constant)  or  the  variation  space  for  N  is  fixed  (Nn  <  N  <  NITT„„, 
where  N  and  N  are  constants) ,  the  requirements  for  storing  of  the 

IjUW  HIGH 

covariance  matrix  can  be  restricted  to 


P,  =  {P. 


k-N. 


HIGH 


k_NHIGh+1"' 


,P 


k-N. 


LOW 


(3.84) 


The  requirements  concerning  storing  of  the  observer's  velocity  and  ac¬ 
celeration  will  also  depend  on  the  representation  of  the  tracking  problem. 
Equations  (3.79) -(3.83)  therefore  represent  an  upper  bound  on  the  memory 
capacity  requirements. 

Intuitively,  if  the  observer  has  performed  a  maneuvre  during  the 
time  interval  Uk-N)T,  kT] ,  a  global  iteration  scheme  will  improve  the 
observability.  The  idea  is  that  we  then  get  crossbearings  over  tliis 
time  interval. 

Three  different  approaches  to  the  Global  Iterated  Filter  will  be 
outlined  in  the  following. 
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3.6.1  Cartesian  Coordinate  System  Representation 

The  first  approach  to  be  Global  Iterated  Filter  utilize  the  Cartesian 
Coordinate  system  representation  described  in  Section  3.1. 

The  number  of  iterations,  i,  on  each  set  of  data,  has  to  be  limited. 
The  following  test  is  proposed  in  order  to  stop  the  iteration  sequence 
at  a  time  instant  kT: 


(3.85) 


where  the  £  vector  has  to  be  selected  through  simulations  as  a  compromise 
between  accuracy  and  computertime  (number  of  iterations) . 

If  a  target  maneuvre  is  detected,  the  size  of  the  iteration  interval 
should  be  decreased  in  order  to  not  perform  iterations  on  premaneuvre 
target  data.  The  iterator  should  therefore  have  an  adaptive  calculation 
sequence  for  N,  in  connection  with  the  maneuvre  detection  system.  We 
will  return  to  this  point  when  dealing  with  the  maneuvre  detection  system 
in  Chapter  5. 

The  iterator  will  work  in  the  following  way: 

Having  performed  iterations  on  the  observation  sequence  {z. 

K— 4&N 

Zk-2N+l . W’  m’til  the  °riteriUB  18 

satisfied,  we  have  the  state  vector  x^_N  ^_N  and  the  covariance  matrix 
P  .  .  These  are  stored  in  the  computer  memory.  Perform  the  global 

iteration  algorithm  subsequently  on  the  observation  sequences  {z^  2Mll' *  *  *  * 

Zk-N+1^'  ^Zk-2N+2'*"'  zk_N+2^  etc'  At  each  fulfille<J  iteration,  say 
at  samplepoint  jT,  the  covariance  matrix  P^  ^  and  the  state  vector 

x.  .  are  stored.  Like  in  the  conventional  Kalman-f liter  case,  however, 

-3,3 

only  the  value  of  the  state  vector  at  the  last  samplepoint  is  needed,  so 
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the  next  state  vector  x .  .  can  use  the  same  storage  are  as  x .  ,  .  ,  (and 

~J r3  -j-l, 3-1 

thus  destroy  x .  ,  .  , ) . 

-3-1/3-1 

Eventually,  when  we  arrive  at  sample  kT,  we  have  the  following  data 
base  picture  (older  data  have  been  discarded) : 


Observation  sequence :  { z^  , z^} . 

Covariance  sequence,  {Pk_„  . Vl,k-1! 


Observer  position  sequence:  {x  , ...,x  .} 

"*k-N 


State  vector:  ^  ^  ^ 


From  this  startpoint  the  following  calculations  are  performed: 


1. 

2. 


Time  updating. 


Calculate  x,  .  ,  and  P,  ,  , 
~k,k— 1  k,k-l 


Observation  integration . 

and  P.1  ,  ,  where  i=l. 
k,k 


Process  z,  , 
k 


resulting  in 


k 


3 .  Timebackdating : 


^-H.k-K  '  'f('NT)  4.k 

P,  „  ,  „  =  fetched  from  the  data  base 
k-N,k-N 


(3.86) 


4.  Reprocess  the  observation  sequence,  resulting  in 
$i+l  pi+l 
x,k'  k,k* 


5.  Perform  the  test  described  by  equation  (3.85)  to  decide 
whether  to  continue  the  iteration  loop,  or  to  stop  the 
iterations  on  this  observation  sequence. 


-32- 


a.  If  decision  is  continue  the  iterations,  set  i=i+l, 
and  start  from  step  3  again. 

b.  If  decision  is  to  stop  the  iterations,  store  xf+^ 

i+1 

and  P  in  the  computer  memory,  set  k  =  k+1,  and 

K/K 

start  from  step  1  again. 

This  iterator  will  successively  improve  the  linearization  trajectory 
for  (given  by  equation  (3.10)).  The  utilization  of  the  data  sequence 
therefore  will  be  increasingly  more  optimal. 

3.6.2  Polar  Coordinate  System  Representation 

The  second  approach  to  the  Global  Iterated  Filter  utilize  the  Polar 

Coordinate  system  representation  described  in  section  3.2. 

The  mechanization  of  the  iterator  is  identical  to  the  iterator  in 

sequence  3.6.1,  except  for  the  time-back  dating  step. 

Due  to  the  nonlinearity  of  the  system  dynamics,  where  the  target 

position  information  is  given  in  relative  coordinates  (relative  to  the 

observer),  timeback  dating  by  use  of  the  inverse  form  of  equation  (3.25) 

(with  the  process  noise  vector  v.  =  0) ,  would  be  unnecessary  time- 
P 

consuming,  x.  .  would  then  have  to  be  calculated  for  decreasing  l,  i=k, 
i,i 

k-l,...,k-N,  not  in  one  big  backdating  step  as  we  could  do  for  the  Cartesian 
Coordinate  system  model  (see  equation  (3.86)). 

In  order  to  save  conqputertime ,  the  following  approach  for  time-back¬ 
dating  is  proposed: 

1.  The  state  vector's  position  information  is  transformed  to 
Cartesian  representation  through: 


(3.87) 


-33- 


*k,k  *Sc,k  S^n  ^k,k  +  Xsk 


yk,k  =  Vk  COS  *k,k  +  ysk 


(3.88) 


where  the  different  variables  are  defined  (without  time 
subscript  k,  though)  on  Fig.  3.1.  Since  the  velocity 
representation  is  equal  in  the  two  representations,  we 
now  have  a  complete,  Cartesian  state  vector  x^ 

2.  Perform  time  backdating  through  equation  (3.86),  resulting 
in  ~k-N,k-N. 

3.  Perform  the  transformation  from  Cartesian  to  Polar  repre¬ 
sentation  through  the  equations 


VN,k-N  "  \ 

i  «  V  «  B  tan  T- 
k-N,k-N  \  y 


'k-N,k-N~Ys, 


(3.89) 


k-N 


\-N,k-N  V^txk-N,k-N~Xsk_N)  +  (yk-N,k-N~Ysk_N)  (3.90) 


Now,  if  the  time  back  dating  step  of  the  iterator  described  in 
section  3.6.1  (step  no.  3)  were  changed  to  include  processing  of  equations 
(3.87)  and  (3.88)  before  processing  of  equations  (3.86),  and  to  include 
processing  of  equations  (3.89)  and  (3.90)  subsequent  to  equation  (3.86), 
the  calculation  steps  1-5  given  in  section  3.6.1  will  be  valid  also  for 
the  Polar  coordinate  system  case. 

One  additional  preferable  requirement  exists,  however:  The  data¬ 
base  should  also  contain  the  observer's  incremental  position  change  from 
sample  to  sample,  given  by  the  left  hand  side  of  equation  (3.24),  over 
the  time  interval  [ (k-N)T,  kT3 . 
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Alternatively,  these  quantities  can  be  calculated  from  the  sequences 
given  in  equations  (3.81)  and  (3.82),  if  these  cure  in  the  database. 

This  iterator  will  successively  improve  the  linearization  trajectory 
for  (given  by  equations  (3. 31) -(3. 32) ) .  The  computational  require¬ 
ments  will,  however,  exceed  the  iterator  in  Section  3.6.1. 


3.6.3  "Serial"  filters 

If  the  two  previous  described  global  iterators  process  each  observa¬ 
tion  sequence  {z^  ,  zjc_N+^ '  •  -  •  twice,  which  is  the  minimum  number 

of  iterations,  each  individual  observation  z^  will  be  processed  2N 
times.  It  is  thus  obvious  that  these  iterators  will  increase  the 
computational  burden  very  heavily,  as  compared  to  the  Extended  Kalman- 
filter  case. 

In  an  attempt  to  reduce  the  calculation  load,  and  still  bring  along 
the  advantages  of  the  global  iteration  approach,  a  different  iterator 

roposed  for  the  Cartesian  system  representation.  The  effect  of  this 
in  fact  two  coupled  filters,  running  along  the  target 
.....  „c.ory  with  a  certain  time  delay,  NT,  between  the  filters.  This  is 
the  reason  for  the  name  serial  in  the  heading  of  this  section. 

The  two  serial  filters  are  added  together  in  one  system  description. 


in  order  to  get  a  compact  form.  We  have: 
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T2/2 

° 
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0 

0 

T2/2 
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0 
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lk-N 

0 
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0 

T2/2 

0 

0 

T 

0 

v 

2k-N 

M  j 

0 

0 

1  0 

T 

(3.91) 


*k 

Tt-N 


tain 


tan 


w. 


w. 


k-N 


(3.92) 


k-N/ J 

In  equations  (3.91)  and  (3.92),  N  is  the  number  of  samples  between  the 
two  pairts  of  the  state  vector. 

We  now  define: 


*k 


rx  n 

-ik 

*k  * 

_-2k 

_^k-N 

and 


■ - 1 

k-N  _ 

\  = 


Then,  equation  (3.91)  can  be  written: 


^k+1 


0 

(J)((N+1)T) 

6(T) 

0 

,  <>•■  ■  -  "  i 

<j»(-(N-l)T) 

0 

+ 

0 

0(T) 

(3.93) 


(3.94) 


v*  (3.95) 
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Further  defining: 


the  observation  equation  (3.92)  can  be  written: 


(3.96) 


(3.97) 


(3.98) 


(3.100) 

(3.101) 


(3.102) 


-37- 


<j>(T) 


0 

4>(  (N+1)T) 

<f>(-(N-l)T) 

■» 

0 

J 

(3.103) 


and 


(3.104) 


where  V  and  are  given  by  equations  (3.8)  and  (3.9),  0(T)  by  equation 
(3.6)  and  <J>(T)  by  equation  (3.5).  can  also  be  written: 


\  j  0 

0 1  "k-J 


(3.105) 


where  is  given  by  equation  (3.10). 

The  Extended  Kalman  filter  equations  for  this  augmented  (•)  system, 
are  given  by  equations  (3 .11) - (3.18) ,  if  replacement  with  the  augmented 
variables  are  performed  in  the  equations. 

In  order  to  start  up  the  augmented  system  correctly,  the  first  N 
observations  are  processed  with  the  Kalman-filter  given  in  section  3.1. 
Then  the  initial  values  for  the  augmented  system  will  be: 


2o,-l 


*k,k 


*0 


(3.106) 


•0,-1 


k,k 


Lo 


oJ 


(3.107) 
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At  this  point  it  is  appropriate  with  a  few  comments  on  this 
approach  to  global  iteration.  As  we  can  see  from  equation  (3.95),  the 
first  part  of  the  state  vector,  x^,  is  updated  on  the  basis  of  the 
second  part  of  the  state  vector,  x^,  and  vice  versa.  From  equation  (3.92) 
we  can  further  deduce  that  each  observation  will  be  integrated  twice. 

Looking  at  the  iteration  scheme  given  in  section  3.6.1,  we 
can  see  that  our  ' serial"  filter  is  equivalent  with  the  steps  1-4, 
with  step  4  reduced  to  integration  of  followed  by  a  time  updating 

step  up  to  time  (k+l)T.  Step  5  does  not  exist,  each  observation  is  only 
reprocessed  once. 

The  global  iteration  effect  of  this  approach  should  then  have  been 
demonstrated. 

It  should  be  pointed  out  that  this  scheme  can  be  augmented  to 
incorporate  a  selected  number  of  filters,  say  M,  following  each  other 
with  a  time  difference  NT.  The  resulting  model  would  then  look  like: 


(k+1)  =  ' 

I 

0 


0  4>((N+1)T)  0 

x  s 

-  X  N 

0  v 

1  \  \ 

I  X  \ 

I  X  \ 

^  nn  <M(n*Dt) 

<j>  (-  (MN-l)T)  0 - X  0 


*1 


0  (T)  0 

\ 


. 0  0  (T)_ 


with  an  observation  equation: 
....  n  r.  -l/Ax  \ 


m ®) 


_z(k-MN)J  [tan-1  (k-MN)^J  [_w(k-l 


+  . 


(3.108) 


(3.109) 
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In  this  case,  each  observation  will  be  processed  M  times. 


3.7  Parallel  Filter  Approach 

The  last  approach  that  will  be  explored  is  the  parallel  filter 
approach.  In  this  case,  where  we  have  M  parallel  filters  with  different 
linearization  trajectories,  only  the  simplest  Cartesian  model  for  each 
filter  will  be  considered,  in  order  to  reduce  the  computer  time.  Each 
filter  will,  therefore,  be  represented  by  the  mathematical  model  given  by 
equations  (3.3)— (3.7) . 

The  philosophy  behind  this  approach  is  the  following:  Each  filter 

will  be  initialized  with  different  range,  R  .,  i=l,2,...,M.  The  initial 

oi 

values  for  the  velocity  components  for  each  filter  will  depend  on  RQ, 
if  the  initialization  routine  proposed  in  Chapter  4  is  used. 

In  the  time  interval  from  initialization  until  the  first  observer 
maneuvre,  target  range  is  unobservable.  The  utilization  of  the  bearing 
observations  in  this  time  interval  will  be  nonoptimal  for  all  the  filters 
except  the  one(s)  with  approximately  correct  range.  When  the  observer 
performs  a  maneuvre,  target  range  becomes  observable.  It  is  then  possible 
to  identify  which  of  the  M  filters  that  has  the  most  correct  range. 

The  resulting  state  vector  for  this  filtering  scheme  can  be  given 
by  [14]: 


4  -  ",  p<\i  -  W  •  4i  l3-ll0) 

1=1 


where  the  probabilities  P 
The  question  is  how. 


1=1, 


,M,  have  to  be  calculated. 
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Now,  the  most  likely  quantity  to  contain  information  about  these 
probabilities,  are  the  innovation  sequences  for  each  filter.  These  are 
given  by: 


£ki  zk  ^fk-] 


i=l,2,. . .  ,M 


(3.111) 


In  our  special  case,  c  ^  is  a  scalar  variable.  Under  a  number  of  conditions, 

which  include  the  retirement  of  equality  between  the  physical  system  and  the 

mathematical  model  contained  in  the  Kalman-filter,  the  innovation  sequence 

2 

has  been  shown  to  be  a  white  Gaussian  sequence  with  statestics  ~N(0,  a^) , 

2 

where  a  is  given  by: 


°ki  =  Vk-l^k-lVk-l  +  Wk 


(3.112) 


2 

aki,  i=l,2,...,M,  are  already  calculated  by  the  Kalman  filter  algorithm. 

2 

m^  -  0,  and  a^,  i=l,2,...,M,  represents  the  expected  mean  and  variance 
for  the  innovation  sequence. 

The  expected  variance  of  the  innovation  sequence  given  by  equation 
(3.112)  will  be  different  for  each  filter,  i.e.,  the  variance  will  depend 
on  R^.  Equation  (3.112)  can  be  written: 


4  =  x  '4  t .  •  +  4  v  •sin  4.k-r4  .  --K.k-i’n 

R.  k,k-i  k,k-l  k,k-l 

i 


or  equivalently 


(3.113) 


2  1  i 

Gki  -  2  ^11 

RT  k,k-l 

i 


+  4  „ ,  -  4 +  \ 

k,k-l  i 


(3.114) 


where 
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I  I 


^  _ «L  ,  i  X  _  1  ^  i  1  _  1 

°B.‘  *11,.  ,.  ,  Sln  \,k-]  +  *22,.  „  ;COS  \,k-l  +  V  „  ' 
i  k,k-l  k,k-l  k,k-l 


sin2$. 


k,k-l 
(3.115) 


The  difference  between  the  M  different  <p^'s  will  not  be  significant 
in  this  case,  since  the  observability  of  the  bearing,  <j>,  is  very  high. 

The  values  of  the  covariance  matrix  elements,  P  p22  P12'  ’wever' 

will  depend  on  R. .  In  the  equations  for  calculation  of  P  ,  ,  the  observation 

X  JC  t  JC 

2 

noise  will  be  weighted  by  R^.  See  Appendix  E  for  detailed  derivation  of 
the  covariance  equations . 

As  can  be  seen  from  Appendix  E,  each  of  the  elements  of  the  covariance 

matrix  P,  will  increase  for  increasing  R. .  The  value  of  the  expected 
JC  /  iC  X 

innovation  variance  given  by  equation  (3.113)  will,  therefore,  not  decrease 
-2  2 

as  a  function  of  R.  ,  however,  a,  .  will  decrease  as  some  function  of  R. , 
i  ki  i 

since  the  elements  of  P  are  limited  in  their  growth,  and  does  not  in- 

JC  /  JC 

2 

crease  with  the  power  of  R^  anyway.  A  detailed  simulation  analysis  of  the 

equations  given  in  Appendix  E  has  to  be  carried  out,  in  order  to  reveal 

the  exact  behaviour  of  P,  ,  as  a  function  of  R. . 

k,k  i 


The  actual  statistics  for  each  filter  i,  i=l,2,...,M,  can  be  approximated 


by: 


in 


'ki 


1 

N+l 


j=k-N 


e. . 

31 


(3.116) 


§S 


al  =  nh  E  (eji'V  } 

j=k-N  31 


(3.117) 


The  idea  is  now  to  compare  the  innovation  sequence's  actual  statistics 
with  its  expected  statistics,  and  thereby  get  an  expression  for  the 
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probabiliti.es  PfR^  =  R^/z^) . 

In  doing  this,  we  have  to  consider  the  following  two  cases: 

1.  System  not  observable  in  the  time  interval  [ (k-N)T,  kT] . 

2.  System  is  observation  in  the  time  interval  [(k-N)T,  kT] . 

By  observable  or  not  we  mean  whether  or  not  the  range/velocity 
ambiguity  can  be  resolved  through  the  bearing  observation  over  the  time 
interval  [(k-N)T,  kT] .  In  any  case,  the  bearing  to  target  will  be 
observable,  and  so  will  the  ratio  Av/R,  where  Av  is  the  relative  velocity 
between  target  and  observer. 

The  information  as  to  whether  the  system  is  observable  or  not,  and 
the  degree  of  vhe  observability,  is  known  to  the  tracking  routine, 
since  the  observer's  maneuvering  history  in  the  time  interval  [(k-N)T,  kT] 
is  known. 


3.7.1  System  not  observable. 

If  the  observer's  velocity-  and/or  course-changes  are  below  defined 
thresholds  over  the  time  interval  in  question,  we  know  that  the  range/ 
velocity  ambiguity  cam  not  be  resolved  from  bearing  observations  only. 

This  case  can  further  be  devided  into  the  following  two  sub-cases: 

1.  The  target  is  not  maneuvering  during  the  time  interval. 

2.  The  target  is  maneuvering  during  the  time  interval. 

A  maneuver  is  defined  as  a  definite  course  and/or  velocity  change, 
not  the  natural  small  fluctuations  in  velocity  and  course  due  to  waves. 


wind,  etc. 
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3 . 7 . 1 . 1  Target  not  Maneuvering 

If  the  target  is  moving  along  a  straight  course  with  constant  speed, 
the  actual  mean  value  of  the  innovation  signal  given  by  equation  (3.1165 
will  be  close  to  zero  (below  a  defined  threshold,  say  ) . 

Since  we  know  that  the  target's  natural  fluctuations  about  some 
mean  course  and  speed  doesn't  depend  on  the  distance  from  where  it  is 
observed,  it  is  likely  that  the  bearing  statistics  should  contain  some 
range  information. 

In  fact,  this  case  looks  like  the  stationary  process  case  where 
adaptive  noise  estimation  is  possible.  See  Mehra  [15],  [16],  and  Chin 
[171,  [18]. 


In  this  case  we  propose  to  use  the  covariance  matching  method, 
however,  not  to  do  any  adaption  (even  if  that  should  also  be  possible) , 


but  to  arrive  at  an  expression  for  the  probability  function  p(R.  =  R/z  ) . 

1  JC 

If  we  set  the  equations  (3.114)  and  (3.117)  equal  (expected  variance 
actual  variance) ,  we  get: 


-i  (P1 
2  1  11 

R.  k,k-l 

l 


(R.) 


+  P 


22 


k,k-l 


(Ri}  ' 


+  wk  = 


(3.118) 


'k. 

x 


Solving  this  equation  for  the  only  explictly  occurring  R^  in  the  equation 
gives  us: 


R*  = 
l 


ip*  (r.)  +  p:L  (r.)-<C 

X1k  k-1  1  22k  k-1  Ri 

<4  -%  ’ 

ek.  * 

i 


where  R*  may  be  different  from  R.. 
x  x 


(3.119) 


The  difference: 
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AR.  =  R*  -  R. ,  i=l,2 

1X1 


,M 


(3.120) 


will  be  an  expression  for  the  range  error  for  each  filter. 

We  propose  the  following  form  for  the  probability  function 
p  (ft±=R/ z^.) : 


-a*  Ar2 

p(Ri  =  R/z^_)  =  K  •  e  1 


(3.121) 


where  the  variables  K  and  a  will  depend  on  the  process  noise  covariance 
matrix  V^»  and  have  to  be  decided  upon  through  simulations. 


3. 7. 1.2  Target  Maneuvering 

During  a  target  maneuvre,  the  method  described  in  section  3. 7. 1.1  will 
not  work,  since  we  in  this  case  gets  temporary  changes  of  unknown  size 
and  duration  in  the  process  noise  covariance  matrix  ,  that  are  not 
taken  into  account  in  the  calculation  of  the  expected  variance  of  the 
innovation  signal. 

Therefore,  when  a  target  maneuvre  is  detected,  for  example  by  an 

abrupt  change  in  both  the  actual  mean  and  variance  of  the  innovation 

signal,  the  last  calculated  probabilities  p(R^  =  R/z^)  should  be  used, 

until  the  filters  have  adapted  to  the  new  course  and/or  speed,  and 

m_  again  drops  below  the  threshold  m  , . 

^i 

Alternatively,  if  it  is  possible  to  decide  upon  the  time  delay 
between  the  instant  of  the  target  maneuvre  and  its  detection,  the 
probabilities  P(R^  =  should  be  used,  where  At  is  the  described 

time  delay. 

Target  maneuvre  detection  will  be  treated  in  Chapter  5. 
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3.7.2  System  Observable 

If  the  observer  is  maneuvering  during  the  time  interval  [(k-N)T,  kT] , 
the  range/velocity  ambiguity  can  be  resolved  based  on  the  bearing 
observations  only.  In  this  case  the  filter  with  the  most  correct  range 
can  be  identified,  if  the  target  is  not  maneuvering  during  the  time 
interval  in  question. 

We  therefore  have  to  consider  the  same  two  sub-cases  as  in  section 
3.7.1: 

1.  The  target  is  not  maneuvering  during  the  time  interval. 

2.  The  target  is  maneuvering  during  the  time  interval. 

3. 7. 2.1  Target  not  maneuvering 

The  filter  with  correct  range  will  not  change  its  actual  statistics 

when  the  observer  performs  a  maneuvre.  This  fact  can  be  utilized  to 

prune  off  the  filters  with  wrong  range. 

2 

By  monitoring  m  and  a  given  by  equations  (3.116)  and  (3.117)  through  an 
£k 

observer  maneuvre,  it  is  thus  possible  to  identify  the  filter  with  the 
best  range  as  the  filter  with  the  least  change  in  actual  statistics. 

In  this  case,  we  continue  to  calculate  the  probability  functions 
given  by  equation  (3.121)  for  a  gradually  decreasing  number  of  parallel 
filters,  as  the  filters  with  the  wrongest  ranges  successively  will  be 
removed  from  being  in  an  active  state  to  a  so-called  "dormant"  state, 
where  they  no  longer  are  updated. 

The  selection  of  the  filters  with  bad  ranges  can  be  made  from  monitoring 

m  .  When  m  exceeds  a  given  threshold  m  ,  the  filter  no.  i  is  temporarily 
ki  ki 
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pruned  off  the  ensemble. 

In  this  way  the  number  of  necessary  parallel  filters  can  be  reduced, 
and  if  the  observer  maneuvre  extends  over  a  time  interval  long  enough,  or 
if  the  observer  performs  successive  maneuves  while  the  target  remains  on 
straight  course  with  constant  speed,  we  will  eventually  be  left  with  one 
filter,  with  correct  range. 

This  situatx  n  will  prevail  only  until  a  target  maneuvre  is  detected, 
when  all  the  M  parallel  filters  should  be  initialized  again. 


3. 7. 2. 2  Target  maneuvering 

When  a  target  maneuvre  is  detected,  all  the  M  filters  should  be  re¬ 
initialized,  independent  of  the  number  of  active  filters  at  the  time  of 
maneuvre  detection. 

The  M  filters  should  be  reinitialized  with  different  ranges 
centered  about  the  estimated  range  k,  given  by  the  state  vector 
x^  ^  at  the  time  of  maneuvre  detection.  The  difference  between  the 
individual  R^’s  should  be  much  less  than  during  the  first  initializa¬ 
tion,  since  range  is  not  at  all  so  uncertain  this  time. 

Simultaneously,  the  velocity  elements  of  the  covariance  matrix  should 
be  increased,  in  order  to  allow  the  filter  to  adapt  to  the  targets  new 
course  and  speed. 

After  reinitialization,  the  calculation  of  the  probability  function 
given  by  equation  (3.121)  is  resumed. 

Target  maneuvre  detection  and  handling  in  connection  with  parallel 


filters  are  treated  in  section  5.3. 
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3.7.3  Closing  remarks,  parallel  filters. 

The  parallel  filter  approach  seems  very  promising,  however,  the  calcu¬ 
lation  load  imposed  by  this  approach  will  be  substantial. 

In  order  to  arrive  at  a  practical  parallel  filter  solution,  the 
idea  of  adaptively  reducing  the  number  of  parallel  filters  have  to  be 
investigated  thoroughly.  This  can  only  be  done  through  a  simulation 
study. 

Further,  this  approach  claim  for  a  substantial  simulation  study  in 
order  to  "tune"  in  the  different  thresholds,  time  intervals  and  variables 
defined  in  the  previous  sections. 

One  fundamental  difference  between  the  parallel  filter  approach  and 
the  single  filter  approacl  ,  is  the  range  covariance.  For  the  parallel 
filter  approach  the  range  covariance  should  be  kept  low,  in  order  to 
make  each  filter  "stiff"  in  range.  This  is  not  normally  the  case  for  the 
single  filter,  where  range  covariance  has  to  be  high  in  order  to  allow  the 
filter  to  adopt  the  correct  range  during  the  periods  when  range  is  ob¬ 


servable  . 


4.  INITIALIZATION  ROUTINE 


4.1  General 

Bearing  only  tracking  from  a  single,  moving  observer  is,  in  the 
initialization  phase,  very  dependent  on  the  selected  initial  values  of 
range,  course  and  speed  of  the  target. 

The  initial  values  of  the  elements  of  the  covariance  matrix  for 
the  estimation  error  have  also  an  important  influence  on  the  initial 
track. 

One  method  commonly  used  for  selecting  initial  data  for  bearing 
only  trackers,  is  to  utilize  the  knowledge  of  range  reach  of  the  bearing 
sensor.  The  argument  is  the  following:  If  a  target  is  detected  at  some 
time  Tq,  that  could  not  be  "seen"  by  the  sensor  at  times  <  T^,  the 
reason  is  that  the  target  has  just  entered  the  reach  area  of  the  sensor, 
and  is  opposing  the  observer.  The  initial  range  is  therefore  given  by 
the  sensor  specifications,  and  the  initial  course  is  towards  the  observer 
(Initial  velocity  still  has  to  be  picked  out  of  the  air) . 

Since  the  range  reach  of  the  sensor  usually  depends  on  the  environ¬ 
mental  conditions,  this  method  frequently  will  give  very  bad  initial  data. 

The  purpose  of  this  chapter  is  to  derive  an  alternative  method,  where 
initial  data  can  be  selected  in  a  more  optimal  manner,  based  on  calcu¬ 
lations  performed  on  a  given  set  of  bearing  observations  from  the  moving 
observer,  and  the  position  history  of  the  observer. 

The  following  derivation  will  also  give  us  some  valuable  insight 
in  the  observability  problem,  and  show  the  conditions  under  which  the 
range/velocity  ambiguity  can  be  resolved  based  on  bearing  observations 
only. 
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at: 

3ZS 


4.2  Geometric  Problem  Visualization 
A  typical  geometric  situation  is  depicted  in  Fig.  4.1. 


A  moving  observer,  0,  observes  a  number  of  bearings  to  the  target, 
T,  at  the  time  points  tQ,  t  ,  t^, . . . ,  where  the  time  difference  between 
any  two  consecutive  bearings  may  be  different. 

The  following  assumptions  are  made: 

1.  The  target  is  moving  with  constant  course  and  speed. 

2.  The  observer's  velocity  and  course  are  constant  between 
the  observations.  (This  restriction  will  be  removed 
later) . 

3.  The  bearings  are  noise  free. 

Assumption  number  3  is  obviously  not  true  in  reality.  However,  by 
use  of  this  assumption  it  is  possible  to  arrive  at  certain  results. 
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In  the  discussion  of  these  results  later  in  this  chapter,  methods  for 
reducing  the  effect  of  this  assumption  being  violated  will  be  suggested. 


4.3  Calculation  of  target  velocity  components. 

Based  on  three  consecutive  bearings  <j)g,  <f>^,  cf>2,  and  an  assumed 
start  range,  RQ,  the  target  velocity  components  can  be  calculated.  We 
have  the  follow!  three  equations  (see  also  equation  (3.25)): 


AL^At^ 

VALp(Atl) 


tan  (<!>1-<f>0) 


(4.1) 


4=/ 


(Rn+AL  (At  ) ) 

0  p  1 


(4.2) 


%(At2} 

R.  +  AL  (At.)  =  tan42”^lJ 

1  p  2 


where 


4tl  ■  tl"t0 


4t2  *  V1! 


or  generally 


At. 

l 


=  t.-t 
r 


i-1 


9 


i=l,2 


and 


(4.3) 

(4.4) 

(4.5) 

(4.6) 


AV* 


V 


cos  4>.  ,  [v  At  .-Ax 


]  -  sin  At  -  Ayg  ]  (4.7) 

i-1  *  i-1 


Al  (At.)  =  sin  <|> .  .  [v  At. -Ax  ]  +  cos  <j).  .  Iv  At. -Ay  ]  (4.8) 

p  1  l-l  xi  s.  ,  i-l  y  i  ■'s.  , 


In  equations  (4.7)  and  (4.8) ,  assumptions  1  and  2  from  section  4.2  are 
used.  The  target  movement  along  x-  and  y-directions  in  the  time  inter¬ 
val  At.  are  given  by  v  At.  and  v  -At.  respectively  (no  acceleration 
i  xi  y  i  r 

term) ,  while  the  observer  movement  along  the  same  axis  in  the  time 
interval  are  given  by: 


1  2 
Ax  =  v  .At.  +  -  a  At. 
s .  -  sx.  ,  i  2  sx.  ,  1 

i-l  i-l  i-l 


Ay  =  v  At.  +  q  a  At? 
si-i  syi-i  1  2  syi-i  1 


(4.10) 


If  the  observer  position  increments  in  the  time  interval  Ati  is  calculated 

on  the  basis  of  equations  (4.9)  and  (4.10),  assumption  2  is  necessary. 

However,  by  deviding  the  time  interval  At^  into  smaller  intervals, 

and  calculating  Ax  and  Ay  as  a  summation  of  position  increments 

i-l  Si-1 

over  these  intervals,  provided  that  velocity  and  acceleration  data  for 
the  observer  in  these  intervals  exists ,  the  assumption  of  constant 
velocity  and  course  can  be  changed  to  yield  arbitrarily  small  time 


increments. 


Now,  from  equation  (4.1)  we  have: 


ALN(At1)  =  tan^-iy  (1^  +  AL  (At^) ) 


(4.11) 


Inserting  from  equation  (4.11)  into  equation  (4.2)  gives: 


Rx  =  (RQ  +  AL  (At1))  •  -'l  +  tan  (t^-c^) 


(4.12) 
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Then: 


Rx  =  (Rq  +  Al  (At^/eos^-^) 


(4.13) 


Further: 


RQ+sin  4>Q  (vxAt1"Axs  1  +  cos  4»0(v  At1-Aya  ] 


Rl  = 


COS  (^"^q) 


(4.14) 


Inserting  for  A’^ot^)  and  AL^ (At1 )  in  equation  (4.1),  gives,  after 


P  1 


some  calculation: 


1  1  R0  sin  (W 

v  =  Ax  +  tan  4>-  (v  -  -r—  Ay  )  +  -rr - r — 

Atl  s0  1  Y  *sQ  Atx  cos  <j>1 


(4.15) 


Now,  inserting  for  from  equation  (4.15)  in  equation  (4.14)  gives: 


Rl  = 


Ep  cos  ^  +  (vyAt1-AyS(?) 

COS  4> 


(4.16) 


We  also  insert  for  vx  from  equation  (4.15)  in  the  equations  for 
AL^At^  ar.d  AL^CAt^)  (equations  (4.7)  and  (4.8),  with  i=2) .  The 
results  are: 


&t2  A 2 

N(iV  *  sq  \  ‘  3i”<»rV  +  “s  *i(sq 


■  At 


-  sin 


*i(s7  Ays0  -  ** Ml) 


(4.17) 


At2  A^2 
W  =  R0  tan  V^W  +  sin  ^(  -g-  Axs0-Axgl) 


At, 


cos 


__  v - (— Ay  .  sin2  <$>,,+ Ay  -cos2*  )  (4.18) 

4^  y  cos  (j)^  VAt^  Js0  Y1  •'si  vl/ 
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From  equations  (4.17)  and  (4.18)  we  find  that: 


=  N(At2),tan  +1  +  cosT^  W^sl1 


(4.19) 


Then  we  have: 


R.+AL  (AtJ  = 

1  p  2 


R0  COS<*b  +  ALN(At2)sin<|,1  +  Vy(Atl+At2)“(AysO+AYsl) 


COS  Cj> 


(4.20) 


Inserting  for  (R^+ALp(At2) )  from  equation  (4.20)  in  equation  (4.3)  gives: 
Al  (At  )cosi{> 

*  E0  cos  *0  +  4Vit2)sin  *1  +  V4tl+4t2) 


(4.21) 


-  (Ay  -tAy  , ) 
Jrs0  Jsl 


Since  ALR(At2)  given  by  equation  (4.17)  is  not  a  function  of  v  ,  and 


AV4t2  *  V*0 


(4.22) 


equation  (4.21)  can  be  solved  for  v  .  The  result  is: 


1  Aln ( At2 ) cos  <J>2 


v  =  - — — -  - ; — — — — - R  cos  4>_  +  Ay  +  Ay 

y  t2_to  sin(4>2-<j>  )  0  T0  ■'sO  •'si 


(4.23) 


Inserting  for  Al^fAt^  from  equation  (4.17)  gives  our  final  result  for 


(At  sin (<}>  -<}>  ) cos  <j>0  j 

v  =  -  Rn]~-  - .  4j~~r - —  -  cos  <{._.(  +  Ay  _  +  Ay  .  + 

y  t2-t0  0(At1  sinf^-*^)  0j  sO  ■'si 


COS  ^2  /At2  \  /At2 

lcos  -  ainh[iq  toso-  ysi, 


(4.24) 
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Lastly,  inserting  for  from  equation  (4.24)  in  equation  (4.15),  gives 
our  final  result  for  v^: 


'  V'o 


At2  sin  (4)1-<j>0)sin<(i2 
At, 


s: .  v(|>2-<j)1) 


-  sin4». 


+  Ax  -  +  Ax  + 
sO  si 


sin(J>, 


sin  (<J>2-<t>n 


J  [oos  *i(sE^  ^so'^si)  '  4yso-^)] 


(4.25) 


Equations  (4.24)  and  (4.25)  are  the  main  results  in  this  section.  We 
are  interested  in  their  form  for  a  couple  of  special  cases: 


Case  1: 


At^  *  At2  =  At 


Observer  non-maneuvering,  i.e.: 


Ax  _  =  Ax  ,  =  v  -At 
sO  si  sx 


Ay  „  *  Ay  ,  =  V  *At 
Js0  •'si  sy 


(4.26) 


Q  |  sin(4»1-<ji0)sin(|), 

^At  I  sin(4>2"4>1) 

Rq  jsin  (4^-4^)  cos 
TSt  I  sin(<j>2-(j)1) 


v  =  v  +  — 
x  sx  2 


V  =  V  +  -T 

y  sy  2 


-  sin 


-  cos 


I 


(4.27) 

(4.28) 

(4.29) 

(4.30) 


As  we  can  see  from  equation  (4.29)  and  (4.30) ,  the  target  velocities 
that  can  be  determined  on  the  basis  of  any;  three  consecutive  bearings 
from  a  non-maneuvering  target,  will  depend  on  the  range  RQ  (can  be 
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transformed  to  yield  by  solving  equation  (4.16)  for  after  in¬ 
serting  for  Vy.  Similarily,  the  range  dependence  can  also  be  transformed 
to  R2) . 

It  is  important  to  realize  that  the  same  dependence  on  range  exists 
when  the  target  velocities  are  calculated  by  the  Kalman  filter.  This  is  the 
range/velocity  ambiguity  in  a  nut-shell! 

Case  2; 

Afci  =  At  2  =  At 

Observer  not  moving  at  all,  i.e.: 


„  „  -l/\\  „  -]/»*»  <W' in  <I2-Sin  Sin»o\ 

T  '  tar‘  U-  \sin  1^-^) cos»2- sin (^-^1  cos»0  j  la'J' 

The  target  velocity,  however,  will  still  depend  on  the  range. 

These  two  special  cases  also  can  be  used  to  give  guidelines  for  an 
intelligent  observer  maneuver  strategy:  Start  tracking  while  the 
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observer  stays  put,  and  determine  the  target  course.  When  the  observer 
starts  moving,  preserve  the  determined  target  course  in  the  Kalman  filter 
(by  keeping  a lew,  may  be  artifically) .  Then  the  filter  will  quickly 
adopt  the  correct  range.  An  indication  of  this  can  be  found  from 
equations  (4.29)  and  (4.30).  We  cam  write: 


v  v  +  R*K, 

.  _  x  sx  1 

tan  C_  =  — 

T  v 

y 


'  +  R-K- 

sy  2 


(4.36) 


where  is  assumed  known,  and  and  K2  are  const  amts,  independent  of 
range.  Equation  (4.36)  cam  be  solved  for  R,  to  give: 


R  = 


v  -  v  •  tan  C 
sx  sy _ T 

K • tanc  -  k 
2  T  1 


(4.37) 


4-4  Calculation  of  Initial  Range 

By  inclusion  of  a  4th  bearing  observation,  the  results  of  the  previous 
section  can  be  extended  to  include  the  determination  of  the  initial 
range,  provided  the  observer ^ is  maneuvering  during  the  observation 
period. 

When  a  4th  bearing  is  included,  the  following  equation  can  be 
included: 


wAv 


tan(4>3-<}>2) 


Here,  R2  is  given  by: 


(4.38) 


\/(R1+ALp(At2))2  +(ALN(At2j)2 


(4.39) 
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Since,  from  equation  (4.3): 


Al^tAt^  =  (RX+AL  (At2))tan((p2-({>1) 


equation  (4.39)  can  be  written: 


R2  =  fRl  +  /cos  (<f>2“<t)1) 


Now,  by  use  of  equation  (4.20)  we  get: 


(4.40) 


(4.41) 


R2  = 


R0COS^0  +  ALN(At2)sinj>1  +  vy(At1-t-At2)-(Ays0-tAysl) 

COS  COS(<j>2~<j)j) 


14.42) 


The  next  step  is  to  insert  for  AL^ (At^)  from  equation  (4.17)  and  for  from 
equation  (4.24)  in  equation  (4.42).  After  some  manipulations,  we  get  the 
following  result: 

At„ 


R2  " 


•jgr  R^infc))^^)  +  cos 


*1(4^  tosi)  - sln 


sin(4>2-<{>1) 


(4.43) 


Next,  the  two  other  variables  in  equation  (4.38),  AL^At^)  and  AL^(At^) 
have  to  be  calculated.  We  have: 


AVAt3)  =  COS  *2  lvxAt3  "  Axs2]  "  Sin  tt>2[vyAt3_Ays2] 


(4.44) 


ALp(At3)  =  Sin  VVxAt3  ~  Axs2]  +  COS  <J>2[vyAt3'Ays23 


(4.45) 


|| 

I  ff 

8; 


fe  fer 


tJ# 

m  m 


Further : 


4y4t3>  *  '  ta"*2  +  lVVAy32! 


(4.46) 
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NOW,  inserting  for  AL^fAt^)  from  equation  (4.46)  in  equation  (4.38)  gives, 
after  some  rearrangements: 


cos  4>3 

4V4t3>  ■  sln(»3-»2)'  -  *2  COS*2  +  VSt3  '  4ys2 


(4.47) 


Next,  we  have  to  develop  AL^ (At^)  further.  Inserting  for  and  v  from 
equations  (4.25)  a.  '  (4.24)  in  equation  (4.44)  gives,  after  some  calculation: 


RQAt 

AL^At.j)  =  - sin  (4:2“<l)0)  +  cos  4>2 


At. 


l^(4iS0  +  **sl)-,ats2 


(4.48) 


-  sin<j>„ 


At. 


<4y30+teSl,-AyB2 


L  2  0 


Lastly,  by  inserting  for  Al^At^  from  equation  (4.48),  for  R2  from  aquation 
(4.43)  and  for  vy  from  equation  (4.24),  equation  (4.47)  can  be  solved  for 
rq.  The  result  is: 


R0  = 


sin  (»2H>1) •  At^ sin(j>3  (At3  (AysQ+  ysl)  -  (t2-tQ)  Ays2)  -cos^  (At3  (Axsp4 
At^* At^- sin (4>3~ 4>Q) sin (<ti2“<^)  " 


Axsl)~(t2~t0)AxS2)}  +  sin(V<j>2)(t3-to)  |cos(j.1(At2Axs0-At1Axsl)-sin())1(At2Aye0-At1Aysi; 

At2  (t3-tQ)  sin  (4>1-4>0)  sin  (4>3-<f>2) 

(4.49) 

Equation  (4.49)  is  the  final  result  in  this  section.  An  important  special  case 
is  obtained  when: 

At^  =  At2  =  At^  =  At 
t2-t0  *  24t 


t3-t0  =  3At 


(4.50) 
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Then,  equation  (4.49)  can  be  written: 


Sin  (jy-^  {sin4»3  (Ays0+Aysl-2Ays2)  -cos<f>3  (Axs0+Axsl-2Axs2)  }  + 
sin (4>3-<)>0)  sin (^-(Jy  -3  sin ((J^-c^)  sin (4>3-<j>2) 


3  sin(4>3-4>2)  .  {cos^  (Axs0-Axsl)  -sin<j>1  ^YsQ-^Ysl)  ) 


(4.51) 


If  the  observer  is  not  maneuvering,  we  have: 


Ax  =  Ax  =  Ax 
sO  si  s2 


(4.52) 


4yS0  *  4ysl  ‘  Ays2 


From  equation  (4.51)  it  is  then  easy  to  see  that  RQ=0,  meaning  that  range 
can  not  be  observed  with  a  nonmeaneuvering  observer. 

When  range  UQ  is  calculated  from  equation  (4.49)  or  (4.51),  equations 
(4.24)  and  (4.25)  can  be  used  with  this  range  RQ  to  calculate  the 
target  velocity  components . 

Appendix  F  gives  a  numerical  example  on  the  use  of  the  results  in 
section  4.3  and  4.4. 


4.5  State  vector  initialization. 

Due  to  obvious  reasons,  the  single  Kalman  filter  case  and  the  parallel 
filter  case  have  to  be  treated  separately  under  this  heading. 


4.5.1  Single  Kalman  filter  case. 

The  results  in  Section  4.3  and  4.4  are  based  on  the  assumption  of 
noisefree  bearings.  They  can,  however,  be  used  directly  as  initial  data, 
with  the  associated  initial  values  for  the  covariance  matrix  as  given  in 


section  4.6.3  and  in  appendices  G  and  H. 
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In  the  following,  however,  two  different  methods  will  be  suggested 
in  order  to  improve  the  initial  values  of  velocity  and  range.  The  approaches 
will  depend  on  whether  the  observer  is  maneuvering  or  not  over  the  time 
period  [0,  NT],  which  we  will  call  the  initialization  period. 

f 

We  assume  that  the  following  data  are  available: 


1.  A  sequence  of  bearing  observations,  Z„  =  {<{>,.,  <j>,  .  ,.d> } 

N  0  1  N 


1 


2.  The  observers  position  increments.  Ax  =  {Ax  _,Ax  „...Ax  } 

S  sO  Sl  Sn 

N-l  n-1 

and  Ay  =  {Ay  ,Ay  ,...,Ay  }. 

SN-1  80  S1  SN-1 


Case  1,  Observer  Maneuvering 

Step  1;  Calculate:  Ri=  f($±,  ^i+35 ' 

ftmction  f  is  defined  by  equation  (4.49) . 


. ,N-3.  The 


Step  2:  Calculate:  vxi  =  f^I^,  <j>2+1#  <J>i+2)  »  i=0,l, . . . ,N-3. 

vyi  =  f2(Ri'  ^if  ^i+1'  ^i+2^  •  •  •  »N“3* 

where  the  functions  f ^  and  f2  are  defined  by  equations  (4.25) 
and  (4.24) . 


Step  3:  Calculate  the  initial  velocity  elements  as: 


'x0 


vy0  = 


N-3 

E  v 


"-2  ffo  “ 


N-3 

E  vw. 


N-2  ft  yi 


(4.53) 


(4.54) 


Step  4:  Calculate  the  target  course  as 

-ifw 


CT  -  tan 


yo  > 


(4.55) 
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Step  5:  Calculate  initial  range,  R^ ,  from  equation  (4.37) 


Step  6;  Initial  bearing  is  given  as  4>Q. 


Step  7:  Initial  position  elements  are  calculated  as: 


X0  =  Xs0  +  R0  Sin  *0 


(4.56) 


Yo  =  yso  +  Ro  cos  *0 


(4.57) 


Step  8: 


_  _  t 

Starting  with  Xq  =  [xQ  yQ  v^q]  ,  process  the  bearings 

{<b, ,  cp. , . . . ,  <f> }  through  the  Kalman  filter,  to  give  the 
1  2  N 

resulting  state  vector  at  t  =  t^,  where  the  track  is  offically 
started. 


Case  2,  Observer  not  maneuvering 

In  this  case,  equation  (4.49)  is  worthless,  and  no  range  information 
can  be  subtracted  directly  from  the  bearings.  In  this  case,  the 
following  steps  are  proposed: 


Step  1:  Select  as  the  sensor's  maximum  range.  (If  possible, 
take  environmental  conditions  into  account) . 


Step  2:  Calculate:  vxi  =  f^R^  4^  4>i+1r  <j>i+2) 


V  =  W  V  W  *i+2> 


*i+l  =  W  V  V  W 


i  =  0,1,2, ... ,N-3.  The  functions  f  and  f2  are  given  by 
equations  (4.25)  and  (4.24),  while  the  function  f3  can  be 
found  by  an  extension  of  equation  (4.16),  as: 


! 
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We  assume  that  the  same  data  are  available  as  in  Section  4.5.1, 
and  we  will  have  the  same  two  cases,  depending  on  the  observer's  maneuvering 
scheme : 


Case  1,  Observer  Maneuvering. 

Steps  1-5:  Same  as  in  Section  4.5.1,  case  1. 

Step  6:  With  a  previously  defined  AR^,  calculate  R^,  i=l ,2 , . . .  ,M,  as: 

V  =  Ro  -f  •  *  a  14-63) 


Step  7:  Calculate  the  velocity  components  v  and  v  from  equations 

Oi  Y0i 

(4.25)  and  (4.24),  i=l,2,...,M. 


Step  8:  Initial  bearing  for  each  of  the  M  filters  is  given  as  =  <j>Q, 
i=l,2 , . . . ,M. 


Step  9:  Initial  position  data  are  calculated  as: 

X0i  =  Xs0  +  R0i'Sin  *0  (4‘64) 

Y0i  =  Ys0  +  R0i  COS  ^0  (4‘65) 

i=l ,2 , ... ,M. 

Step  10:  Same  as  Section  4.5.1,  case  1,  step  8  for  each  filter. 

Case  2,  Observer  not  maneuvering 

Steps  1-6:  Same  steps  as  in  section  4.5.1,  case  2,  resulting  in  Rq  at 


the  end  of  step  6. 
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Step  7:  With  a  previously  defined  Ar„  >  AR. ,  calculate  R  . ,  i=l,2, . . . ,M, 

""  a  X  ux 

as: 


Oi 


-  i-AR 


2 


(4.66) 


(Rq  is  assumed  to  be  near  the  maximum  range  for  the  bearing  sensor, 
and  any  RQ^  >  RQ  should  not  be  necessary) . 


Steps  8-11:  Same  steps  as  Case  1  above,  steps  7-10. 


4.6  Covariance  Matrix  Initialization 

The  next  subject  to  be  addressed,  is  the  selection/calculation  of 
initial  values  for  the  covariance  matrix,  PQ.  We  will  only  derive  the 
initial  covariance  matrix  for  the  Cartesian  system  model.  (Necessary 
transformation  to  the  Polar  coordinate  system  case  can  be  done  through 
equation  (3.48)). 

Since  the  philosophy  behind  the  parallel  filter  approach  is  totally 
different  from  the  single  Kalman  filter  approach  with  respect  to  assumed 
initial  accuracy  in  r singe,  these  two  cases  have  to  be  treated  separately 
also  under  this  heading. 

4.6.1  Single  Kalman  Filter  Case 

Three  different  approaches  to  covariance  matrix  initialization  will 
be  outlined  under  this  heading. 


4. 6. 1.1  Aidala’s  Approach 

Aidala  [11]  has  treated  this  subject  very  thoroughly,  however,  his 
model  of  the  target  motion  analysis  problem  has  no  process  noise. 
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Consequently ,  his  time  update  equation  for  the  covariance  matrix  has 
the  form: 


Pk+l,k  =  ^(T)-Pk,k^T) 


(4.67) 

T 


i.e.,  no  increase  in  uncertainty  with  time,  since  the  term  6(T)V^0(T) 
from  equation  (3.18)  is  lacking,  since  his  conclusions  depends  entirely 
on  this  unrealistic  assumption,  his  results  are  not  applicable  directly. 
Aidala  proposes  to  select 


po  =  V1 


(4.68) 


and  shows  this  selection's  superiority  in  terms  of  covariance  matrix 
stability  over  the  selection 


po  = 


0 


v  J 


(4.69) 


By  a  pseudolinear  formulation  of  the  tracking  problem,  obtained 
through  a  nonlinear  transformation,  Aidala  transforms  the  nonlinear 
observation  equation  to  a  linear  equation,  with  the  nonlinearities 
embedded  in  the  observation  noise  (see  also  [7] .  The  measurement 
standard  deviation  for  this  model  turns  out  to  be: 


-  ‘Vk-i'V  • 

k 


(4.70) 
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and  this  is  the  only  term  containing  R  and  0Q  in  the  covariance  equations 
for  the  pseudo-linear  filter. 

The  covariance  matrix  of  the  pseudolinear  filter,  P^  is  related 
to  the  covariance  matrix  of  the  original  filter,  P  ,  in  the  following 

JC  /  Jv 

way,  if  equation  (4.68)  is  used  for  initialization: 


By  selecting  0Q  »  RQ,  equation  (4.70)  can  be  approximated  as: 

0  =ywk  (4.72) 

K 

and  the  covariance  equations  for  •  the  pseudolinear  filter  is  completely 

independent  of  range  and  0Q (PQ  Q  =  I) . 

However,  the  reason  why  this  form  is  possible,  is  the  form  of  the 

covaricince  time  update  equation  given  by  equation  (4.67).  If  equation 

2 

(3.18)  was  used,  the  initial  value  of  the  variance,  oQf  can  not  be 
collapsed  into  the  measurement  variance  term,  as  given  by  equation 
(4.70). 

Now,  is  it  feasible  to  adapt  Aidala' s  results  for  our  model,  where 
time  updating  of  the  covariance  matrix  is  performed  according  to  equation 

(3.18) ? 

This  question  is  not  possible  to  answer  without  performing  simula¬ 
tions. 

For  the  case  with  a  nonmaneuvering  observer  during  the  initialization 
phase,  it  is  therefore  suggested  to  incorporate  the  selection  of  the 


initial  covariance  matrix 
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P0  =  R0  *  1 


(4.73) 


as  one  possibility  to  be  explored. 

4. 6. 1.2  General  Approach 

The  most  commonly  used  initial  covariance  matrix  is  probably  of  the 


form: 


P  tc  P  = 

0,-1  0 


P11  P12 


P21  P22 


0 

0 


P33  P34 


0  P43  P44 


where : 


(4.74) 


Pn  =  cos^-^.cy2  +  sin24>0.02 


(4.75) 


P12  =  P21  =  COS  Vin<J,O*[0Ro-(ROa(j»o)2] 


(4.76) 


P22  =  S±n2<|)O*(RO04»o)2  +  C°s2V0Ro 


(4.77) 


P33  =  cos  2V<V0Co)2  +  sin2cO‘0vo 


(4.78) 


P34  =  P43  =  Sin  C0*COS  COt0vo-(V0co)2] 


(4.79) 


P44  =  sin2cO(VO0co)2  +  COs2co’0Jo 


(4.80) 


The  usual  (ad  hoc)  selection  of  parameter  values  in  equations  (4.75) 


(4.80)  is  the  following: 
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<j>0  =  the  observed  bearing  at  time  tQ 

0.  =  the  sensor's  standard  deviation 

R  =  the  sensor's  max  range. 

0 

a_  »  i  (the  sensors  max  range) 

Ro  2 

co  "  -*o 

V.,  0  and  0  :  completely  picked  out  of  the  air.  As  an  example ; 

C0  V0 

Vq  =  10  m/sec 

0  =  30° 

C0 

0  =  20  m/sec. 

V0 

4. 6. 1.3  Suggested  Approach 

If  the  state  initialization  methods  given  in  section  4.3  and  4.4 
are  used,  a  better  initial  covariance  matrix  can  be  obtained. 

Like  in  the  state  vector  initialization  case,  the  approach  will 
depend  on  the  observers  maneuvering  scheme: 

Case  1,  Observer  Maneuvering. 

Step  1:  Range  can  now  be  calculated  from  equation  (4.49).  Calculate 

2 

the  initial  variance  for  range,  0  ,  through  the  approach  given 

R0 

in  Appendix  G. 

Step  2;  The  velocity  elements  can  be  calculated  from  equations  (4.24) 

and  (4.25) .  Calculate  the  velocity  elements  of  the  covariance  matrix 
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through  the  approach  given  in  Appendix  H. 

When  the  "smoothing"  approach  given  in  section  4.5.1  is  used,  the 
resulting  initial  values  of  the  state  vector  should  be  better  them  the 
resulting  initial  covariance  matrix  from  this  approach  will  indicate „ 
However,  since  we  are  interested  in  an  initially  "open"  filter  (a 
filter  that  responds  to  the  measurements) ,  we  suggests  not  to  take 
this  into  account.  If  the  actual  accuracy  obtained  through  the  methods 
given  in  section  4.5.1  turns  out  to  be  much  higher  than  the  resulting 
covariance  from  this  approach,  it  will,  however,  be  worthwhile  to  take  the 
"smoothing"  effect  into  account  also  for  initial  covariance  calculations. 

4.6.2  Parallel  Filter  Case 

With  M  filters  running  in  parallel,  we  are  interested  in  each 
filter  being  as  "stiff"  as  possible  in  range,  since  it  is  likely  that 
one  of  the  filters  have  an  initial  range  close  to  the  correct  one,  as 
will  subsequently  become  apparent  through  an  observer  maneuvre  (see 
section  3 . 7  and  4.5.2). 

For  the  parallel  filter  approach,  the  only  difference  between  the 
two  cases:  observer  maneuvering/not  maneuvering,  is  the  size  Ar  between 
the  initial  values  of  range  for  each  filter  (see  section  4.5.2,  AR^  or 
ta2>. 

The  following  approach  is  suggested: 

Step  1:  Select  C„  = 

R* 


Ar 
2  ' 
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Step  2:  Select  0,  =  the  sensors  standard  deviation. 

*0 

Step  3:  Given  the  initial  range  i=l,2,...,M,  calculate  the 

position  covariance  elements  for  each  filter  from  equations 
(4.75)-(4.77) . 


Step  4:  Calculate  the  velocity  elements  of  the  covariance  matrix 
for  each  lilter  through  the  approach  given  in  Appendix  H. 


5.  MANEUVRE  DETECTION  AND  HANDLING 


Up  to  this  point  we  have  assumed  constant  course  and  speed  for  the 
target.  This  assumption  will  now  be  removed,  and  we  will  allow  our 
target  to  make  abrupt  changes  in  course  and/or  speed,  of  random  sizes, 
and  occuring  at  random  time  instants. 

Several  different  approaches  to  the  maneuvering  target  tracking 
problem  have  been  proposed  in  the  literature.  We  will  in  the  following 
give  a  short  survey  cf  some  of  the  most  important  approaches. 

Jazwinski's  [25]  limited  memory  filtering  approach  is  probably 
the  simplest  approach.  By  preventing  the  covariance  matrix  elements 
from  decaying  below  certain  thresholds,  resulting  in  filter  gains  above 
certain  values,  the  target  state  vector  dependence  on  the  latest  ob¬ 
servations  are  increased.  However,  the  tracking  performance  of  this 
approach  during  nonmaneuvering  periods  of  the  target  will  decrease 
due  to  higher  dependence  on  the  observation  noise. 

The  natural  solution  to  this  problem  is  to  model  the  target  under 
the  nonmaneuvering  hypothesis,  and  in  addition  to  ir Produce  some  adaptive 
maneuvre  detection  scheme  which  can  step  in  and  give  the  filter  limited 
memory  for  a  short  period  of  time,  after  a  maneuvre  is  detected  [4], 

[20]. 

A  further  extension  to  this  approach  is  suggested  by  Willsky  and 
Jones  [26] ,  [27] .  They  suggest  the  possibility  of  simultaneously  with 
the  detection  of  an  abrupt  system  change,  to  estimate  the  size  of  the 
change  and  to  perform  a  state  variable  correction  directly,  in  addition 
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to  give  the  filter  limited  memory  temporarily.  Similar  approaches  are 
outlined  in  [9]  and  [10] . 

Another  avenue  along  which  many  researchers  have  been  working,  is 
to  model  the  target  maneuvres  as  a  semi -Markov  process,  whereby  N  possible 
acceleration  inputs  are  selected  according  to  some  a  priori  probabili¬ 
ties.  In  its  pure  form,  this  approach  requires  an  infinitely  growing 

2 

bank  of  parallel  'liters,  N  initially,  N  for  the  second  measurements , 
etc.  Different  approaches  to  reduce  this  described  growth  are  proposed 
ir.  the  literature,  in  order  to  get  practical,  realizable  filters 
[14],  [10],  [21],  [22],  [28],  [291. 

Another  interesting  approach  was  proposed  by  Tenney  et  al.  [24] . 

Two  extended  Kalman  filters  are  operating  in  parallel,  one  with  a 
large  artificial  system  noise  covariance  term  added  to  give  the  filter 
limited  memory  and  thereby  allowing  it  to  track  fast  maneuvres,  and 
the  other  filter  with  small  artificial  noise,  making  this  filter  re¬ 
stricted  to  tracking  of  constant  course/speed  trajectories.  Maneuvre 
detection  is  performed  by  comparing  the  behaviour  of  the  two  filters. 

None  of  the  approaches  to  maneuvering  target  tracking  resumed 
above,  deal  with  a  tracking  scheme  based  on  bearing  only  information. 

Due  to  the  low  observability  of  the  system, maneuvre  detection  under 
these  circumstances  are  more  difficult.  Most  of  the  papers  above  claim 
for  high  observability  in  order  to  make  efficient  maneuvering  target 
trackers . 

Another  important  fact  to  realize  is  the  possibility  to  separate 
the  maneuvering  target  tracking  problem  into  two  subproblems s 
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1.  The  maneuvre  detection  problem. 

2.  The  maneuvre  handling  problem  (i.e..  When  the  maneuvre 
is  detected,  what  actions  should  be  taken  to  allow  the 
tracker  to  adapt  to  the  new  target  course/speed) . 

This  separation  is  imbedded  in  the  approaches  given  in  [4]  ,  [9]  ,  [24]  , 

[26]  [27] ,  and  will  also  be  used  in  the  approach  proposed  in  this  report. 

The  lacking  observability  during  time-periods  when  the  observer 

is  nonmaneuvering,  results,  as  we  have  seen  in  Chapter  4,  in  the 

range/velocity  ambiguity.  Unless  special  preventing  actions  are 

taken,  the  Kalman  filter’s  reaction  to  a  target  maneuvre  can  be  a  range 

jump  as  well  as  a  course/velocity  jump.  The  obvious  preventing  action 

2 

is  to  keep  the  range  variance  a  low,  forcing  the  filter  to  adapt  course/ 
velocity  as  a  maneuvre  detection  reaction,  and  leave  the  range  to  target 
unchanged.  This  seems,  however,  only  feasible  to  do  if  we  have  arrived 
at  a  stable  target  track  with  correct  range  prior  to  the  maneuvre.  If 
the  target  maneuvre  takes  place  while  the  tracker  still  is  in  the  initiali¬ 
sation  phase,  with  a  poor  linearization  trajectory  in  range,  our  wish 
is  to  keep  a  high  range  variance  in  the  Kalman  filter,  so  that  the 
filter  easily  can  arrive  at  correct  range  if/when  the  observer  performs 

a  maneuvre,  and  range  becomes  observable. 

2 

These  conflicting  preferences  on  c  are  among  the  reasons  why 

R 

maneuvering  target  tracking  is  harder  to  solve  for  the  bearing  only 
measurement  case  than  for  cases  with  complete  observability. 

One  possible  way  to  resolve  this  conflict,  is  to  make  use  of  our 
knowledge  of  the  observers  position  history  and  future  maneuvre  in¬ 
tentions.  If,  for  the  global  iterated  filter  case,  the  observer  has  not 
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performed  maneuvres  during  the  iteration  interval,  and  generally  for  all 

the  filter  approaches,  if  the  observer  is  not  going  to  start  maneuvering 

2 

in  the  near  future,  it  is  no  point  in  keeping  a  high,  since  range  can't 

R 

be  observed  from  the  observations  in  any  case.  However,  we  will  get  a 
possible  conflict  if  the  observer  and  the  target  are  maneuvering  at 
the  same  time.  The  solution  to  this  problem  has  to  be  decided  upon 
through  simulations. 

A  proposed  approach  to  maneuvre  detection  and  handling  will  be 
given  in  the  following. 


5.1  Maneuvre  Detection 

The  most  powerful  and  best  theoretical  fundamented  approach  to 
maneuvre  detection  seems  to  be  the  generalized  likelihood  ratio  best 
described  in  [30] .  This  approach  has  been  used  by  Willsky  and  Jones 
[26]  ,  [27] ,  by  Tenney  et  al.  [4] ,  and  is  also  suggested  by  Maybeck 
[23]. 

Following  the  approach  given  in  these  references,  two  hypothesis 
on  -che  form  of  the  innovation  signal  can  be  assumed: 


V  £k  vik 


(No  maneuvre) 


H^s  ek  =  “fc  +  maneuvre  has  occurred.) 


where  is  a  zero  mean  white  sequence  with  variance: 


°vlk  ”  Wis-A  +  Wk 


(5.1) 


If  we  restrict  our  attention  to  a  "data  window"  containing  the  N 
most  recent  observations,  our  generalized  likelihood  ratio  test  can  be 
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given  by : 


JV  C.  .  <•' 

-T  .I* 


i=k-L  %.  H1 


where  c^,  L  and  y  are  design  values  which  has  to  be  decided  upon  through 
simulations,  c^  is  a  (possible)  varying  term  independent  of  the  observed 
residual  values.  If  we  define: 


Ylk  Ck“Y 


the  likelihood  ratio  test  can  be  given  by 


*  e2 


i=k-L  V, .  H 
li  0 


(5.4) 


The  reasons  why  we  incorporate  the  possibility  of  having  a  varying 
threshold  y  are  the  following: 


1.  We  want  to  inhibit  maneuvre  detection  during  initialization 
phase . 

2.  We  want  to  inhibit  successive  maneuvre  detections  during  the 
same  maneuvre. 

3.  We  want  to  inhibit  maneuvre  detections  during  maneuvering 
phases  of  the  observer,  if  we  have  reason  to  believe  that  our  lineari¬ 
zation  trajectory  prior  to  the  observer  maneuvre  is  poor. 


The  moaning  Of  equation  (5.4)  is  obvious:  The  actual  variance  of  the 
innovation  signal  is  compared  with  its  expected  value  over  the  most 
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2 

recent  L  samples.  If  becomes  consistently  larger  than  predicted  over 
the  selected  "time  window",  n  target  maneuvre  is  detected. 

A  few  guidelines  can  be  given  concerning  the  selection  of  the  design 
parameters  L,  c^  and  y  (alternatively  L  and  y  ) : 

L  has  to  be  selected  as  a  compromise  between  fast  detection  and  the 
false  detection  rate.  The  longer  time  window  the  slower  maneuvre  detection 
and  the  less  probe— ility  for  false  maneuvre  detection.  The  shorter  time 
window,  the  faster  maneuvre  detection,  but  at  the  same  time,  the  higher 
probability  for  false  detections. 

Y  can  be  decided  upon  by  looking  at.  the  case  with  stationary 
circumstances:  Tracking  of  a  target  going  with  constant  course  and 
speed,  where  our  linearization  trajectory  is  approximately  correct. 

With  =  0  and  the  process  noise  covariance  matrix  vk»  the  observation 
noise  variance  W  and  the  "time  window"  (decided  by  L)  given,  y  can  be 

K 

selected  to  give  a  false  alarm  probability  close  to  zero. 

c^  has  to  depend  on  a  number  of  parameters,  and  may  have  different 
size  and  time  dependence,  depending  on  the  state  of  the  filter.  We 
may  suggest  the  following  structure  of  c^: 

I  -a  (k-k. )  T 
K  e  1  k  >  k 

(5.5) 

0  k  <  kx 

where  T  is  the  sample  time,  and  k^  is  the  sample  when  a  certain  event 
takes  place,  like: 

1.  Initialization 

2 .  Maneuvre  detection 

3.  Starting  of  an  observer  maneuvre 


The  parameters  K  and  a  are  event-dependent  parameters  which  has  to  be 
selected  through  simulations  in  order  to  give  acceptable  false  alarm 
probability  in  the  transient-period  following  the  actual  event.  These 
parameters  will  also  depend  on  the  "time  window"  LT,  on  V  ,  w  ,  P  and 

K  K  U 

on  possible  direct  manipulation  on  the  covariance  matrix  following  a 
detected  maneuvre. 

The  maneuvre  detection  approach  described  in  this  section  should  be 
suitable  for  all  the  filtering  approaches  given  in  Chapter  3.  Whether 
it  is  the  best  approach  to  maneuvre  detection,  independent  of  the  filtering 
approach,  is  unknown. 

For  the  iterated  filtering  approaches  given  in  Chapter  3.4  to  3.6, 
we  would  suggest  to  include  an  the  simulation  study  an  investigation  on 
which  value  of  the  innovation  signal  that  should  be  used  in  equation  (5.4) , 
the  first  or  the  last  iteration  value. 

For  the  parallel  filter  case,  we  propose  to  run  the  maneuvre  detection 
algorithm  only  on  one  filter,  namely  the  filter  which,  at  each  time  in¬ 


stant  kT,  has  the  highest  probability  function  p(R^  =  R^/z^) 


5 . 2  Actions  Following  a  Detected  Maneuvre 

As  described  in  the  beginning  of  this  Chapter,  the  most  appropriate 
action  to  take  when  a  maneuvre  is  detected,  is  to  give  the  filtering 
algorithm  limited  memory  temporarily,  allowing  the  filter  to  adapt  to  the 
new  target  course  and  speed  more  easi ly. 

One  important  fact  to  realize,  however,  is  that  there  is  a  certain 
time  delay  between  the  beginning  of  a  maneuvre  and  its  detection.  This 
time  delay,  say  At,  depends  on  a  number  of  factors,  however,  some  nominal 
value  of  At  should  be  possible  to  obtain  through  simulations. 
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If  we  don't  take  this  time  delay  into  account,  and  impose  limited 
memory  on  the  filtering  algorithm  only  from  the  time  instant  kT,  when  the 
maneuvre  is  detected,  the  observations  taken  during  the  intermediate 
period  from  the  occurrence  of  the  maneuvre  and  its  detection,  will  not  be 
utilized  optimally.  The  result  will  be  an  accumulated  range  error,  that 
can  not  be  driven  to  zero  unless  the  observer  performs  a  maneuvre  after 
the  target  maneuvre. 

In  order  to  reduce  this  range  error,  even  for  the  case  of  a  non¬ 
maneuvering  observer  subsequent  to  a  target  maneuvre,  the  following  steps 
are  suggested: 

1.  When  a  maneuvre  is  detected,  fetch  a  stored  version  of  the 
state  vector  and  the  covariance  matrix  valid  at  time 
(kT-At)  from  the  computer  memory. 

2.  Give  this  time  version  of  "he  filter  limited  memory  by  increasing 
the  velocity  elements  of  the  covariance  matrix. 

3.  Re-integrate  all  the  observations  taken  in  the  time  interval 
[kT-At,  kT] . 

The  result  of  this  iteration  will  be  a  discrete  jump  to  a  more 
correct  position  and  velocity  for  the  state  vector  at  time  kT,  the  same 
time  instant  when  the  target  maneuvre  was  detected.  The  effect  of  this 
approach  is  thus  the  same  as  achieved  by  Willsky  and  Jones  [26] ,  [27] , 
and  estimate  of  the  size  of  system  change,  and  a  direct  correction  of  the 
state  vector.  The  methods  are,  however,  different. 

The  prize  that  has  to  be  paid  for  this  achievement,  however,  is 
substantial.  The  following  time  sequences  has  to  be  stored  in  the  computer 


memory: 
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Polar  coordinate  system  case)  through  the  following  equations: 

2  2  2 
a  =  p  *sin  c  -  (P-.+P^-isinc  cos  c  +  p  .cos  c  (5.11) 

V  34  *ii3  44 

2  12  2 
Oc  =  —  IP33cos  c  +  (P34+P43) sin  c  cos  c  +  P44sin  c]  (5.12) 

v 

Following  a  target  maneuvre  detection,  we  now  want  to  increase 
a  by  A a  and  o  c,  Ao  .  Then,  if  we  define: 

C  C  V  V 


6v  =  (a  +Aa  )2-o2  (5.13) 

V  V  V 

6c  =  (0  +Aa  )2  -  a2  (5.14) 

c  c  c 

the  result  on  the  velocity  elements  of  the  covariance  matrix  will  be: 


2  2  2 

P-,o  P.,.,  +  v  cos  c  •  6c  +  sin  c  •  6v 

33  33 

2 

+  sin  c  cos  c[<Sv  -  v  6c] 

34  34 

2 

p.,  p._  +  sin  c  cos  c[6v-v  6c] 

43  43 

2  2  2 

p.„  ■*"  P..  +  v  *sin  c-6c  +  cos  c*6v 

44  44 


(5.15) 

(5.16) 


(5.17) 


(5.18) 


5.2.2  Actions  Dependent  on  the  Filtering  Approach 
The  design  parameters  in  the  maneuvre  detection  algorithm,  and  the 
actions  following  a  detection,  may  very  well  depend  on  the  tracking 
approach,  i.e.,  the  mathematical  models  of  the  system  dynamics  and  the 
observation,  and  the  version  of  the  filter  equations.  A  "tuning"  of  the 
maneuvre  detection  algorithm  to  each  filtering  approach  may  therefore 
be  necessary,  and  special  actions  following  a  maneuvre  detection  may  be 
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necessary  to  get  the  best  possible  performance  for  each  filtering  approach. 

In  the  following  we  will  discuss  some  of  the  actions  that  may  be 
necessary  (and  feasible)  to  take  for  the  global  iterated  filters,  and 
for  the  parallel  filter  approach.  Only  simulation  results  can  tell, 
however,  whether  further  "tuning"  of  parameters  or  special  actions  can 
give  better  performance  of  an  individual  filtering  approach,  so  this  topic 
will  not  at  all  be  exhausted  by  the  following  discussion. 

5. 2. 2.1  Global  Iterated  Filters 

The  selection  of  the  process  noise  covariance  matrix  is  among 
the  parameters  that  influence  the  "covariance  level"  of  the  filter,  and 
thereby  the  size  of  the  elements  of  the  gain  matrix  K^.  in  fact,  the  degree 
of  limited  memory  can,  to  a  certain  extent,  be  controlled  by  V^. 

Generally,  is  decided  as  a  compromise  between  tracking  performance 
when  the  target  is  going  on  straight  course  and  speed,  and  the  filters 
ability  to  track  small  maneuvres  (without  alerting  the  maneuvre  detection 
and  handling-system) . 

Now,  for  the  global  iterated  filters  described  in  sections  3.6.1  and 
3.6.2,  a  better  tracking  performance  can  be  obtained  during  nonmaneuvering 
periods  of  the  target  with  larger  values  of  the  elements  of  the  process 
noise  covariance  matrix  V  The  reason  for  this  is  the  smoothing  effect 
that  is  a  resut  of  the  iterations,  tending  to  stabilize  the  target  velocity 
vector  when  the  target  is  moving  with  constant  course  and  speed. 

At  the  same  time,  larger  values  on  the  elements  of  will  allow  the 
filter  to  follow  small  target  maneuvres  better. 

As  a  result  of  this,  however,  the  maneuvre  detection  problem  may 
turn  oui.  to  be  more  difficult,  since  higher  values  of  the  elements  of  the 


c 
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gain-matrix  will  tend  to  decrease  the  value  of  the  innovation  signal 
during  target  maneuvres ,  at  the  same  time  as  the  expected  variance  of 
the  innovation  signal  will  increase  due  to  higher  values  of  the  elements 
of  the  covariance  matrix  (see  equation  (5.1)).  An  obvious  result  will 
be  that  different  values  of  the  design-parameters  L,  y  and  c^  have  to  be 
found . 

If  the  itei  ion  interval  [ (k-N)T,  kT] ,  i^rior  to  the  maneuvre 
detection,  is  larger  than  the  interval  tkT-At ,  kT] ,  where  At  is  the 
nominal  time  delay  between  the  occurrence  of  a  maneuvre  and  its  detection, 
i.e.,  if  NT  >  At,  an  obvious  action  to  take  is  to  decrease  the  iteration 
interval  temporarily  in  such  a  way  that  it  is  contained  in  [kT-At,  kT] . 
(Premaneuvre  observations  should  not  be  taken  into  account  when  post- 
maneuvre  course  and  speed  calculations  are  performed) .  Since  the  ob¬ 
servations  taken  in  the  time  interval  [ (kT-At) ,kT]  comes  from  a 
maneuvering  target,  it  might  even  be  interesting  to  look  at  the  possi¬ 
bility  of  deviding  the  iteration  interval  [(k-N)T,  kT] ,  N  =  At/T4  into 
subintervals,  so  that  the  observations  contained  in  each  sub-interval 
are  more  consistent  with  the  constant  course  and  speed  hypothesis  on 
which  the  mathematical  model  of  the  system  dynamics  are  built.  A  reali¬ 
zation  of  this  idea  in  the  global  iteration  context  could  be  a  iteration 
interval  [ (k^-N^)T,  k^T] ,  where  N^  <  N,  starting  with  k^-N^  =  k-N,  and 
finishing  when  k^  =  k  (a  sliding  iteration  "window"  over  the  greater 
iteration  interval  [(k-N)T,  kT] ) . 

The  "serial"  filter  approach,  which  is  a  special  case  of  the  global 


iterated  filter  (see  section  3.6.3),  will  also  need  some  special  treatment 


First,  maneuvre  detection  should  be  made  on 
innovation  signal  at  time  k,  e^.  (As  can  be  seen 
this  approach  has,  formally,  two  observations, 
consequently  two  innovation  signals,  and  ^  ) 


the  basis  of  the 
from  equation  (3.96) 
and  z^_N,  and 


s 


When  a  target  maneuvre  is  detected,  we  propose  to  reinitialize 
the  filter.  It  is  assumed  that  we  have  NT  >  At  (this  is  one  of  the 
design  criterias  for  N) .  The  reinitalizing  sequence  will  consist  of 
the  following  steps: 

1.  Initialize  a  single  Cartesian  Coordinate  System  filter 
with 


|4 

S' 


*0  =  *2k  -  — k-N 

and 


(5.19) 


P  =  P 
0  22k, k 

where  P  is  the  lower  diagonal  part  of  the  co- 

u  6A  f 

variance  matrix  for  the  serial  filter,  given  by: 


(5.20) 


llk,k 

P12k,k 

21k, k 

P22k,k 

(5.21) 


Each  of  the  submatrices  in  equation  (5.21)  are  4  by  4 
matrices . 


2.  Run  this  single  filter  from  time  (k-N)T  up  to  time 
(kT-At) .  (Time  kT  is  the  time  when  the  maneuvre 
was  detected) . 


i 

I 
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3.  At  time  (kT-At),  impose  limited  memory  on  this  single 
filter  as  described  in  section  5.2.1.  For  reasons  of 
convenience  of  notation,  we  now  define 


k2T  **  kT-At 


(5.22) 


4. 


Then  after  increasing  the  velocity  elements  of  the  covariance 

matrix  as  described  in  section  5.2.1,  the  resulting  value  of  the 

covariance  matrix  at  sample  k.  is  given  by  P  .  The  state 

*2 

vector  is  given  by  x,  . 

2 

Run  the  single  filter  from  time  k2T  up  to  time  kT,  where  a 
discrete  jump  in  the  position  and  velocity  at  time  kT  will 
be  the  result  (as  compared  to  x  ^  of  the  serial  filter  at 
time  of  maneuvre  detection) . 


5.  Continue  to  run  the  single  filter  on  new  observations  up  to 
time  (k2+N)T.  New  the  full  serial  filter  can  be  initialized, 
with  the  following  initial  values: 


p 

k2+N 

0 


(5.23) 


and 


So 


(5.24) 


5. 2. 2. 2  Parallel  Filters 

As  was  proposed  in  section  3.7,  the  number  of  parallel  filters  can 
gradually  be  reduced  from  M  towards  1  as  the  filters  with  unlikely  ranges 
are  being  pruned  off.  It  is  therefore  obvious  that  we  have  to  consider 
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two  separate  cases  for  maneuvre  detection  with  this  approach,  namely: 


A.  Maneuvre  detection  when  the  filter  with  correct  range  is 
still  not  recognized. 

B.  Maneuvre  detection  when  only  the  filter  with  correct  range 
is  updated. 

In  order  to  save  computertime  in  Case  A,  we  propose  to  run 

maneuvre  detection  algorithm  only  on  one  filter,  namely  rise  filter  which, 

at  each  time  instant  kT,  has  the  highest  probability  function  p(R^  = 

i 

(Intuitively,  it  don't  seem  likely  that  one  of  the  parallel  filters  will 

be  better  in  identifying  a  maneuvre,  since  range  is  not  observable.  If 

0  is  kept  low,  however,  any  of  the  filters  should  be  able  to  detect  a 
K 

target  course/speed  change) . 

For  the  parallel  filter  approach,  it  seems  likely  that  we  can  give 
up  our  claim  to  take  the  detection  delay  At  into  account,  without  degra¬ 
dation  in  tracking  performance,  so  this  will  be  proposed.  The  reasons 
why  this  is  possible,  are  the  following: 


1.  After  maneuvre  detection,  independent  of  case  1  and  2  above, 
we  intend  to  reinitialize  all  the  M  filters,  with  different 
ranges  centered  around: 

Case  A:  The  range  calculated  from  the  state  vector 
given  by  equation  (3.110). 

Case  B:  The  range  given  by  tl.e  remaining  filter. 

An  eventual  accumulated  range  error 
due  to  the  detection  delay  At,  will 
then  be  picked  up  by  one  of  the  re¬ 
initialized  filters. 
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In  case  A,  the  range  for  all  the  remaining  filters  may 
be  wrong  anyway,  even  at  the  time  when  the  maneuvre  started. 

If  the  time  delay  should  be  taken  into  account  in  case  A, 
we  would  have  to  store  the  state  vector  and  the  covariance 
matrix  for  (in  the  worst  case)  all  the  M  filters  over  the 
last  N  samples,  where  NT  >_  At.  This  would  claim  for  a  lot 
of  comonter  memory  capacity. 
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each  filter's  initial  range  can  be  calculated  from 
equation  (4.63) ,  (substitute  AR2  for  AR^) ,  and  the 
initial  position  data  subsequently  from  equations 
(4.64)  and  (4.65). 

Since,  in  this  case,  the  range  difference  between 
each  filter  is  smaller,  the  remaining  filter's  velocity 
components  can  be  used  as  initial  values  for  all  the 
M  filters. 

Concerning  the  re-initialization  values  for  the  M  covariance  matrices, 
we  propose  to  consider  two  possibilities: 


1.  Use  the  initialization  procedure  described  in  section  4.6.2. 


2. 


Use  the  covariance  matrix  for  the  remaining  filter  (case  B) , 

=  R/z^ (case  A ) ,  but 
modify  its  elements  in  the  following  way: 


or  for  the  filter  with  highest  p(6^ 


12, 


,  2.  2  ,_2  2 

+  cos  4>*0.  •  (R. -R-) 

$0  1  ° 
-  sin  <j>Q  cos 


P 


21. 

x 


p2i  -  sin  <frQ  cos  $Q-02  (R^-Rq) 
0  0 


P22i  *  *220  +  si"2*o-0*0(Er8o> 

where  RQ,  pn  ,  p12  ,  p21  ,  p22  and  4>0  are  referring 

to  the  center  filter,  and  R^,  p^  ,  p  ,  p2^  and 

x  x  '  i 

P22  ,  i  =  1,2,...,M  are  the  initialization  values  of 
i 

the  position  elements  of  the  covariance  matrices  for 
the  M  parallel  filters. 

In  addition  to  this  ,  impose  limited  memory  on  the 
filters  by  increasing  the  velocity  elements  of  the  co- 
variance  matrices  as  described  in  section  5.2.1. 
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Which  of  the  two  proposed  methods  for  covariance  matrix  re¬ 
initialization  that  should  be  selected,  has  to  be  decided  upon  through 


simulations,  as  the  procedure  giving  the  best  result 


6.  SIMULATION  GUIDELINES 


The  development  of  filtering  approaches,  initialization  routines 
and  maneuvering  target  tracking  approaches  given  in  the  preceding  chapters, 
have  to  be  verified  and  compared  through  an  extensive  simulation  study. 

Each  algorithm  should  be  optimized  with  respect  to  design  parameters 
like  process  noise  variance,  maneuvre  detection  parameters  and  actions,  etc., 
before  comparison  between  the  different  approaches  can  be  performed. 

In  order  to  arrive  at  correct  conclusions,  the  target-observer 
geometry  used  in  the  simulations  are  extremely  important .  On  the  other 
hand,  in  order  to  reduce  the  number  of  simulation  runs  to  an  acceptable 
level,  only  a  few  different  geometries  should  be  considered. 

The  simulation  study  should  further  focus  on  two  different  problems: 

1.  The  initialization  phase,  each  filters  ability  to  achieve 
correct  range  estimate  as  fast  as  possible. 

2.  Maneuvre  detection,  ability  to  track  a  target  through 
different  maneuvres  {small/large  course/speed  changes) . 

The  simulation  results  should  be  visualized  on  plots  giving  range 
error,  velocity  error  and  course  error  as  a  function  of  time.  x,y-plots 
should  also  be  provided. 

It  is  believed  not  to  be  necessary  to  run  Monte  Carlo  simulation 
studies  on  all  the  10  different  approaches  given  in  chapter  3.  A  few 
single  simulation  runs  for  each  filter  approach  for  a  small  number  of 
target-observer  geometry  should  reveal  which  approaches  are  worth  further 
study. 

The  most  promising  2  or  3  approaches  should  then  finally  be  compared 
through  Monte  Carlo  simulations. 
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APPENDIX  A 


LINEARIZATION  OF  f (xfc)_ 
We  have 


f(^i  - 


**  +  tan 


'/\V +  *4 


xk 


yk 


We  intend  to  calculate 


'<4>  - 


3f 


<£> 


3f 


_JL 

3<J> 


3f  3f 

3r~  37“ 


3v 


3f„  3f„  3f„  3f. 


2 

3<j) 


‘2 

3R 


3v  3v 


(Al) 


(A2) 


In  order  to  be  able  to  calculate  the  different  elements  of  F(x^),  the 
following  derivatives  are  needed: 


3Hk 

9»k 


(A3) 


(A4) 
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OAL  . 

s*r  ■ 


(AS) 


3\ 


3v 


=  0 


(A6) 


xk 


3vyk 

3%c 


=  o 


(A7) 


3v 


=  T  *  cos<}>. 


(A8) 


xk 


3Al 

7xk 


’Tsin»k 


(AS) 


3ZV 

377"  =-Tsin\ 

yk 


(A10) 


3Al 


3v 


£* 


yk 


=  T  cos<}>. 


(All) 


Next,  we  define  the  following  two  variables: 

^xk  *  «k  +  filpk)3in  *k  +  ^Nk  •  c0s  *k 

V  “  <Kk  +  i%k)C°S  *k  '  %k  Si"  *k 
Then  we  have: 


4i 


vw> 


(A12) 

(A13) 


(A14) 


'WfWWI 
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3f, 


al. 


Nk 


•*'  <« 


3f_ 


i  T 1  (vv cos  Wsin  v  T  ^ 


3v 


xk 


4+i 


'Ll 


3f 

3v 


1  ,  -T[(Bk4ALpk)sin  *k  +  Nk  COS  V  -TArxk 


yk 


Ll 


Ll 


3f 

w 


2  \  *  %c 


Vl 


3f2 


3*k  \+l 

3f2  T  [  ( VALpk)  si?  *k  *  ^kcos  T  •  ^xk 


3v 


xk 


*k+l 


\+l 


tAl 


3f2  TtR^cos  \  +  ALNksin  V  = 


3v 


yk 


\+l 


\+l 


(A15) 


(A16) 


(A17) 


(A18) 


(A19) 


(A20) 


(A21) 


Equation  (A14)-(A21)  can  now  be  inserted  in  eq.  (A2) ,  and  we  have  our 
final  result: 


jr 

vv*v 

-%k 

TAL  , 
yk 

-tAl  , 

xk 

2 

2 

2 

2 

lpi«- 

**+1 

\+l 

^+1 

*k+1 

1 

# 

m 

it 

*w* 

fa 

VNk 

Vfba 

TiLkk 

T4lyk 

H 

tt 

^+1 

\+l 

^+1 

\+l 

nt 

0 

0 

1 

0 

1 

«esf 

0 

0 

0 

1 

(A22) 


APPENDIX  B 


CALCULATION  OF  THE  ADDITIONAL  TERMS  C,  AND  L  FOR  THE  SECOND  ORDER 
GAUSSIAN  POLAR  COORDINATE  SYSTEM  MODEL  FILTER. 


1.  CALCULATION  OF  c, 
-k 


c  .  =  trace 
ki 


3 

r3f .  1 

i 

T 

P 

*4 

X 

*  k,k 

,  isl f *  * .4 


(Bl) 


Now,  [3f^/3x^]  has  already  been  calculated  in  APPENDIX  A.  We  have: 


'<4>  - 


i£i 

Ifa 

44 


W'V 


4+i 


w 


\+i 


-Sk 

TAL  , 
_ X* 

-tAl  , 
xk 

4i 

4+i 

4+i 

V^k 

\+i 

\+i 

\+l 

0 

l 

0 

0 

0 

l 

(B2) 


Next,  we  define: 


FI  = 


a 

w 

T 

3 

K 

'44 

WV/!W 

~ALNk/\+l 


l.-T4VS>t+l 


(B3) 
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£124  -  4-  [RL  •  sin  K  *  24VV 

®k+l 


fl31  "  “\*fl24 


£132  -  4  [Bk+l  '  “S  \  +  24Lyk(\+ALpt>1 

Vi 


2T  AL  ,  AL  , 

fi  «  _ x2s_j* 

33  4 

®k+l 


fl34  "  4-  +  2ALyk  1 


\+l 


fl41  *  ®k  ’  fl23 


fl42  “ 


4-  '4i sin  *k  -  “^'W 

Vi 


fi 


T2  ,„2 


43  “  4  tKk+l  "  ^xk^yk1 

\+l 


fl44  *  -fl33 


f2H  =  "  „3 


k+1 


+  ALNk  *  *k] 


£2X2  =  ^  K+1  -  %k  • 

Vi 


(B12) 


(B13) 


(B14) 


(B15) 


(B16) 


(B17) 


(B18) 


(B19) 


(B20) 


(B21) 


(B22) 
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f2l3  = 


t*£+1  cos  ♦fc  '  ALNkk'xk1 


\+l 


(B23) 


TR  2 

f214  * J  [\+lsin  h.  +  ^^yk1 

Vi 


(B24) 


«21  -  -T-  lAV**  "  V%k(\  +  V1 
Vi 


(B25) 


al: 

f2  =  - 
22  -3 


Nk 


k+1 


(B26) 


f2  -  -J-  [RLl  Sin  ‘  (VALpk)ALxk1 

*k+l 


(B27) 


£2,4  -  -r  !4+i  sin  \  •  ‘v'V'V 


_24  <A 


(B28) 


f231  f213 


(B29) 


f232  f223 


(B30) 


£2 


33  -  -y-  [4i  -  AlL] 

Vi 


(B31) 


T  AL  .  -AL 


f2  - - 

34  R3 

Vl 


(B32) 


f2„.  =  f2 
41  14 


(B33) 


f242  "  f224 


(B34) 


f343  -  f234 


(B35) 


f2  = 

44 


t2[rL  - 


(B36) 


Now,  having  calculated  all  the  elements  in  the  two  matrices  FI  and  F2, 
the  vector  c,  can  be  calculated  from  equation  (Bl) .  The  result: 

K 


E  fi..*p. . 
i.j-i  31  13 


l  f  2  . .  P .  . 
c,  =  .  .  ,  31  13 

Ht  i,3=l 


(B37) 


where  P. .  is  the  (i,j)  element  of  the  covariance  matrix  P  .  . 
13 


2.  CALCULATION  OF 


The  matrix  L^  is  defined  by: 


1  2  ?  2 

1^  =  i  (3  fP  1  f) 


(B38) 


In  order  to  be  able  to  calculate  L^,  we  first  define  te  two  matrices  S 


and  T: 


4  4  4 

E  fl-.P..  £  fl  ,P._ _ £  fl  P.„ 

i=l  11  11  i=l  11  12  i=l  11  14 


s  =  1  ' 


1=1  41  ll 


E  fl.-p.. 
.  ,  4i  i4 
1=1 


(B39) 


£  f  2  p  . 
.  ,  ll  ll 

1  =  1 


£  f  2  .  P  .  . 

.  ,  ll  l 4 

1  =  1 


£  f  2  P .  . 

.  ,  4i  ll 

i=l 


£  f2..P.. 
.  i  4i  i4 
1  =  1 


(B40) 


Then  the  4  nonzero  elements  of  the  matrix  L^  are  given  by: 


5-11  =  2  trace  (S-S) 


CB41) 


&12  =  —  trace  (S.T  ) 


%21  =  ~  trace  (T.S) 


(B43) 


&22  =  J  trace  (T.T  ) 
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Finally,  the  matrix  is  then  given  by: 


4  4 

l  s.  .s.  .  E  s  .t 

j,i=l  31  13  j,i-l  31  13 

4  4 

l  t  s.  .  £  t . .t. . 

3,ti  ^  ^  j#i=i  31  13 

\  =  2 


(B45) 


0  0 


APPENDIX  C 


ITERATED  EXTENDED  KAIMAN  FILTER.  DETAILED  EQUATIONS. 

The  Extended  Kalman  filter-equations  for  the  Cartesian  Coordinate 
system  representation  of  our  tracking  problem  are  given  by  equations 
(3.11)-(3.18) .  Equations  (3.13)  and  (3.14)  are  obtained  from  the 
following  equation  for  the  linearized  system: 


54,k '  54,k-i  -  \(5vV4,k-i> 

where 

64,k  -  4,k-4 

54,k-i  -  +(T)-64-i,k-i 

6\  *  vVk-i 


(Cl) 


(C2) 

(C3) 

(C4) 


- 


(Cl) 


The  Extended  Kalman-filter  equation  (3.13)  is  developed 
through  the  special  selection  of  linearization  trajectory,  namely: 


2fc  =  2k,k-i 


(C6) 


When  the  observation  z^  is  processed,  x^  ^  is  obtained,  and  the  system  is 
relinearized  about  x^  Then,  after  processing  of  z^  and  relinearization, 

<Sx^  ^  =  0,  and  also  6x^  =  0  in  view  of  equation  (C3) .  As  we  can  see, 

equation  (Cl)  reduces  to  equations  (3.13)  and  (3.14). 

However,  the  Extended  Kalmanfilter  does  not  utilize  the  improved 
linearization  trajectory  x^  ^  for  the  processing  of  the  observation  z^. 
This  is  done  by  the  iteration  scheme  described  by  Fig.  Cl.  The  e-vector 
has  to  be  decided  upon  through  simulations. 
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APPENDIX  D 


m 


m 


ITERATED  LINEAR  FILTER-SMOOTHER.  DETAILED  EQUATIONS. 

This  iteration  scheme  is  adapted  to  the  Polar  Coordinate  system 
representation  of  our  tracking  problem,  where  the  observation  equation  is 
linear.  That  means  some  simplifications  imposed  on  the  iterator  proposed 
by  Jazwinski  [2] . 

In  order  to  avoid  the  matrix  invertion  necessary  in  Jazwinski "s 
approach,  the  equation 


A 


£i+i  *  +  S(k-  V^i+rSiM-i,*1 


(Dl) 


can  be  transformed,  making  it  unnecessary  to  calculate  S(k,  e^)  explicitly. 
We  have: 


s(k-  v  -  pk,kF(k'  ei)pkii,k 


A 


— i+1  ^c+l,k  +  K(k+1'  ei5  tzk+r^c+l,k] 


where 


K (k+1 ,  e.)  -  *k+1,kHT(HPk+li)HT  +  Vl)-1 


Now,  if  we  define: 


(D2) 

(D3) 


(D4) 


L (k+l ,  £.)  -  HT(H!>ktlj]HT  +  wk+1)'1 


(D5) 


which  gives: 


K(k+1,  e.)  =  Pk+1#kMk+l,  e.) 


(D6) 
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equation  (Dl)  can  be  written,  inserting  for  n  ^  and  s(k,  e^) : 


Si+1  =  4,k +  pk,kF(k'£i)L{k+1'ei)  [zk+r2k+1,k] 


(D7) 


The  resulting  iterator  is  sunsnarized  on  Fig.  Dl. 


STOP 


YES 


FIG.  Dl.  ITERATED  LINEAR 
FILTER-SMOOTHER 


APPENDIX  E 


DETAILED  DERIVATION  OP  THE  COVARIANCE  EQUATIONS 
FOR  THE  CARTESIAN  KALMAN  FILTER. 


In  this  Appendix  we  intend  to  derive  the  scalar  equations  for  the  a 

priori  and  the  a  posteriori  covariance  imbedded  in  the  matrix  equations 
(3.18)  and  (3.16). 

The  purpose  ^s,  if  possible,  to  assimilate  a  deeper  understanding 
of  the  Kalman  filter  mechanism  generating  the  expected  variance  on  the 
innovation  signal. 

In  the  following,  we  intend  to  utilize  our  knowledge  of  symmetry 
in  the  covariance  matrix  to  reduce  the  number  of  scalar  equations  from 
16  to  10  equations. 

Under  these  circumstances  equation  (3.16)  reduces  from  the  Joseph 
form  to  the  form: 


Pk,k  {I'KkVPk,k-l 


(El) 


where  and  is  given  by: 


(E2) 


Hk  = 


cos  4>k  sin  <j>k 


0  0 


(E3) 


The  variance  of  the  innovation  signal  is  given  by: 


°k  ‘  ¥k,k-i"I +  \ 


(E4) 


By  inserting  for  ^  from  equation  (E3)  and  utilize  the  symmetry  fact 

of  the  covariance  matrix,  equation  (E4)  can  be  transformed  to: 
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2  1  2  2 
o k  =  -tP1;L  COS  <j)  +  P22sin  ♦  -  P12  sin2<^  +  wk 


where  the  subscript  k,k-l  are  dropped  on  the  elements  of  the  covariance 
matrix,  and  on  R  and  <j>.  The  same  is  done  in  the  following. 

Now,  equation  (E2)  can  be  written  as: 


\  "  ^k-A  ‘  a2 


Next  we  define: 


Nk  =  R  * 


2  .  _2 


Then  the  elements  of  the  K,  -matrix  can  be  calculated  to  be: 


Klk  *  i£"  tPU  005  +-P12  Sin,f’1 


^k  '  ^  IP12  “S  *-P22  Sin  *' 


K3k  "  ^  IP13  003  ^~P23  Sin  ^ 


(E10) 


Klk  *  i£IP14  005  *-P24  Sin  *> 


(Ell) 


Now,  from  equation  (E6)  we  have: 


Vk,k-1  =  °k-  Kk 


(El  2) 


since  the  covariance  matrix  is  symmetric. 

Then,  inserting  from  equation  (E12)  in  equation  (El)  we  ge' 


Pk,k  Pk,k-1  "  °k  *  *k  ’  *k 


(E13) 
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Based  on  equation  (E13)  and  equations  (E7)-(Ell)  we  get  the  following 
10  scalar  covariance  equations: 


Jc,k  _  k,k-l  1  fTS  .  _  .  .,2 

Pn  -  Pu  -  r  [pu  c°s  *-*12 sin  ♦’ 

k 


(E14) 


=  Pk'k-1  -  ^~[Pn  cos  <|>-P12  sin  4>]  [P^  cos  <j>-P22  sin  <j>]  (E15) 


s 

k,k  k,k-»  1 


P  =  P 
13  *13 


^  [P13  cos  4>-P12  sin  4»]  [P  cos  <J>  -  P23  sin  <f>l 

3  (E16) 


Pk'k  =  Pk^k  1  -  i^-Pn  cos  ^“Pi2  Sin  4)1  [P14  COS  ^"P24  Sin  41  (E17) 


_k,k  k,k-l  1  r_  .  .  .,2 

P22  *P22  -5-  CP12  cos  *1 


22 


(E18) 


P23k  '  "  i£[P12  COS  4  •  P22  Sin  43  CP13  C°S  4~P23  Sin  41 


(E19) 


P24k  =  P24k_1  ”  jf^p12  COS  4>“P22  Sin  43  tP14  COS  4“P24  Sin  4,1  (E20' 
k  z 


.,kfk  _k/k-l  1  .  ,  _  . 

P33  "  P33  -«T  P13  COS  *'P23  S1"  ♦’ 

k 


(E21) 


P34k  =  P34k_1  "  K~  [P13  C°S  4>‘P23  Sin  4)1  [P14  C°S  4“P24  Sin  4,1 
k 


(E22) 


k,k  k,k-l  1  _  , ,  2 

P44  *  p44  '  57  PJ4  ros  *'P24  sin  *> 

4 


CS23] 


In  equations  (E14)-(E23) ,  most  of  the  variables  on  the  Wight  hand  side 


of  the  equations  have  got  their  subscript  k,k-l  dropped. 
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Now,  the  only  place  the  range  R  enters  the  equations  (E14)-(E23),  is 
through  N^.  From  equation  (E7)  and  (E5)  can  be  given  by: 


2  2  2 
N  =  P  cos  <}>  +  P  sm  <j>-P  sin  2(j»  +  R  W, 

JC  XX  22  X  2  K 


(E24) 


Equivalently,  if  we  define: 


2  2  2 
0R  =  Pn  sin  <j>  +  P22  cos  (j)  +  P  sin  2<t> 


(E25) 


equation  (E24)  can  be  written: 


*■*  ‘  P11  +  *22  -  4  +  E\ 


(E26) 


Equations  (E14)  through  (E23)  gives  the  a  posteriori  covariance  after 
an  observation  intergration,  based  on  the  a  priori  covariance  prior  to 
the  observation  integration. 

Timeupdating  of  the  covariance,  i.e.,  calculating  the  a  priori  co- 
variance  at  the  next  sample,  based  on  the  a  posteriori  variance  at  the 
current  sample,  is  performed  through  equation  (3.18).  By  multiplying 
out  this  matrix  equation,  utilizing  the  symmetric  properties  of  the 
covariance  matrix,  we  get  the  following  10  time  updating  equations: 

Pll1,k  =  Pllk  +  2T  P13k  +  t2  P13k  +  <T4/4>*vlk  (E27) 


k+1  ,k 

12 


+ 


T  (P 


k,k 

23 


+  P 


k,k. 
14  1 


+ 


t2p 


k,k 

34 


(E28) 


p£1'k  =  pnk+  T4k+  raV2).vlk  (E29) 

*K1,k  -  ♦  *  4k 


(E30) 
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APPENDIX  F 

CALCULATION  OF  INITIALIZATION  VALUES  FOR  RANGE 
AND  VELOCITY.  NUMERICAL  EXAMPLE. 


In  order  to  demonstrate  the  use  of  the  results  from  section  4.3  and 
4.4,  the  following  example  is  constructed: 


In  Fig.  FI  we  have: 
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R.  «5  5000m 

V 

=  v  =0  m/sec 

0 

sxo 

SX1 

v  =  20  m/sec 

X 

V 

syo 

=  vsy^  =  10  m/sec 

o 

II 

> 

V 

=  -10  m/sec 

y 

sx2 

At  =  25  sec 

V 

=  0  m/sec 

S*2 

Based  on  this  data,  we  can  calculate  Axg Ay^  ,  i  =  0,1,2,  and  <Jk  , 
i  =  0,1, 2, 3.  We  i.ave: 


4*s0  *  ta,l  =  iys2  *  ° 


Ay  „  =  Ay  .  =  250  m 
■‘sO  Jsl 


Axg2  =  ~250  ® 


*o  “0' 


1  *  tan_1^50bo-250  ]'  6'01< 

if  500  1  ~ 

[5000-500  J 

if  1750  1  ~ 

[500-500  J 


4> 

4>2  =  tan 
<J>  =  tan 


12.53° 


21/25° 


Equation  (4.51)  then  gives 


= 


500  sin  (<^-4^)  [sin  4>3*cos  4>31 


0  sin  (4>3_<(>q)  sin -3  sin^-fyj)  sin(4>3~4>2) 


s  5001.54  m 


Prom  equation  (4.25)  and  (4.24)  we  obtain: 


x  ( sin  (4^-4^)  sin 

=  50  '  R0  \  sin (♦,-♦) 


-  sin  4>, 


>)= 20- 


01  m/sec 


APPENDIX  G 


CALCULATION  OF  INITIAL  VARIANCE  FOR  RANGE,  0, 


If  range,  RQ,  is  calculated  from  equation  (4.49),  the  initial  value 
2 

of  a  can  be  calculated. 

R0 

We  assume  that  the  different  bearing  observations  are  statistical 

2 

independent  and  uncorrelated,  with  variance  0^.  The  bearing  observations 
are  further  statistical  independent  and  uncorrelated  with  the  observers 
position  increments,  Ax^,  Ay\  ,  i=0,...,2.  The  different  position  in¬ 
crements  are  also  assumed  statistical  independent  and  uncorrelated,  with 

2  2  2 

variance  0^=0  =  a  . 

xs  ys  s 

We  now  define: 


4xsl)  -  (t2-t0)tes2l 

(Gl) 

4ysl>  -  (VV4ys2! 

(G3) 

>-4Vy.i> 

(G4) 

Now,  by  use  of  equations  (G1)-(G4),  equation  (4.49)  can  be  written: 

sin^-cj)^)  [yl*sin<{>2~  xl'cos  +  sin  ($3-<j>2 )  [x2  cos  <j>^-y2  sinty^] 


* 


0  At,At3  sin(<j>  -(J^Jsint^-^J-At.j  (t  -tQ)  .  sin  ((J^-ij^) -sin  (4>3-<j>2) 

(G5) 


As  we  can  see,  equation  (G5)  can  be  expressed  as: 
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R0  =  I(4*0»*i**2,*3,x3L,x2,yl,y2) 


<G6) 


If  we  develope  the  Taylor  expansion  for  R  about  some  nominal  value 

0 

Rq»  given  by: 


<G7) 


*0  *  4*1'  4>2'  ^3*  xl»  x2»  yl»  J2) 


and  neglecting  u  ■'ms  higher  than  first  order,  we  get: 


-0  ■  *o  *  %  «*0  +  +  W2  w,  +  |3«3  - 


We  now  define: 


^  3f  ,  ^  3f  - 

+  3?/yl  +  W2  Sy2 


(G8) 


6R0  =  R0“R0 


We  then  have: 


^  -  *{6Ro} 


(G9) 


(G10) 


By  making  use  of  equation  (G8) ,  (G9)  and  (G10)  we  get: 

te  '  fe  "> j'  " 


Now,  naming  the  demoninator  in  equation  (G5)  D,  and  the  nominator  N, 
we  calculate  the  different  derivatives  of  the  f-function.  We  have: 
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- -  •  [-At x  At3  cos^-tjjgjsin^-^)  +  At2 (t3-tQ)cos (^-^q) 


sin(4>3-<f>2)  J 


(G12) 


9f 

1 

D2 

N-  i 

3f 

-  -1, 

a*2 

2 

D 

N- 

3f 

1 

3*3 

2 

D 

N-  (At^t  sm(<f>3-<J>0)  cos  ((^h^)  +  At2(t3-t0)cos(<J)1-4i0)sin((j)3-({)2))] 

(G13) 


N-(At1’  At3  sin(<|>  hJi^cos^-^)  +  At2  (t3-tQ)  sin  ((f^^Jcos  (4>3~<J>2) )  ] 

(G14) 


N  (At1At3cos  (<t>3-4>0)  sin  -At2  (t3-tQ)  sin  cos  )  1 

(G15) 


3f  -1  ■  /X  A  \  A 

9x1  =  ~  D  am -cos 


(G16) 


Ifl  =  ^  sin(*2-^)sin^3 


|1_  =  I  sin  O^)  cos*, 


(G18) 
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|§2=“  |  sin  sin  ^ 


Further  we  have: 


4- 

f/Sxl  \2 

LK0)+ 

/3xl  \2 

l^sJ 

4  - 

&ti  •  K 

+  At2  +  ( 

4- 

(ati_\s  + 

(M  .  \2 

Q 

><  to 

I-1 

II 

*>*3  *  ♦  tv 

4- 

73x2  \2 

\3Ax  _  )  + 
L'  SO  ' 

/  3x2  \2 

l3Axsi/J 

4- 

(M-  \2  + 
_\3Ayg0/ 

/.3y2  \2 

Uysi/  J 

+  UAx  „)' 


s2' 


2  2 
•-2-V  las 


,2.2 


*  al  “  <Vto,2(Ata4Ati)0i 


a2  »  (t_-t  )2(At2+At2)a2 

8  o  0  2  1s 


We  now  define: 

3  u.  \2 


K1 


‘  i=0 


and 


k2  •  a-  = 


3f  _ 
*2  *2 


Then  equation  (Gil)  can  be  written 


(G19) 


(G20a) 


(G20b) 


(G21a) 


(G21b) 


(G22) 


(G23) 


(G24) 


J2  (G25) 
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0R  =  K1  °l  +  K2  °2  (G26) 

«Q  t  s 

The  variable  K2  can  be  calculated  to  be: 

k2  =  -T  Csin2 (<J>2-<{,1)  (2At2  +  (t2-tQ)2  +  sin2(V^)(Vt0)2(At2+At2)] 

(G27) 

Further,  K1  can  be  expressed  as: 

Kl  =  [N2K3  -  2N-D-K4  +  D2K5]  (G28) 

D 

where  K3,  K4  and  K5  are  given  by: 

K3  =  2{At2At2[cos2(<j>3-(j)0)sin2(<j>2-^1)  +  sin2  cos2  ] 

+  At1At2At3  (t3-tQ)  sin2  (4>1+4>3-4>0-<^2) 

+  At2(t3“t0)2[cos2(^l“^0)sin2(^3“^2)  +  sl^^oJcos2^^)]} 

(G29) 


K4  =  At1At3{2(yl  sin^-xl  cos<}>3)  sin  (4>3— cos2  (4>2~ + 

[yl  cos<J>3  +  xl  sin4>3 3  cos  sin2  “  [x2  cos$  -y2  simj^]  • 

cos  (4,3-<l>2)  Sin  (<J)1+4>3“4>q— 4>2 )  +  tx2  Sin<t>1+y2  cos^]  sin  (4>3~<{>2)cos  (<j>2~(J>1) 

sin  > 

+  At2  ^3^0)  ^  tyl  sin^'xl  cos^Jcos^-ij^sin^^-# 


-tyl  cos<j>3  +  xl  sin<J>3]  sin{<{)2-(j)1)sin((()1-<|)0)cos(4)3-({>2)} 


(G30) 
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APPENDIX  H 


INITIAL  VALUES  FOR  THE  VELOCITY  COMPONENTS 
OF  THE  COVARIANCE  MATRIX 


From  section  4.3,  equations  (4.24)  and  (4.25),  we  have  the  following 
expressions  for  the  target  velocity  con$>onents: 


x  <At2  sin((|>;L-<J)0)sin(j)0 

vx  “  t2-tQ  R0*At1  sin (4>2-<J,1) 


-  sin  V  r  Axs0  +  ^sl  + 


sin4>_  At  At 

— — tv — t— .  jcos  Ctt —  Ax  .-Ax  ]  -  sin<J>  [v—  Ay  -Ay  .] 
sinfq^-q^)  «  rl  At^  sO  si  r-At^  ■‘sO  •‘slJ 


1  T  lAt2 

-to  _R°  sint^-^) 


vy  t, 


-  cos<j>0}  +  Ays0  +  Aysl  + 


COS  <j) 
sin  (<p  ■ 


kj~  t“S  *1  4*sO-Axsl,-sin*ltTE7‘  4ys0'  ysJ}] 


The  following  accuracies  are  assumed  defined: 


o  =  o  =0 

sx  sy  s 


0.  = 

<l>0  ♦  2  <p 
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Now,  in  order  to  simplify  the  equatiions  to  be  developed,  equations 
(HI)  and  (112)  can  be  written: 


VX  =  fl(Axsi'  Aysi'  V  *0'  *1'  V  (H4) 

Vy  =  f2^Xsi'  Aysi'  R0'  *0'  *1*  V  (H5) 

where  i=0,l. 

We  now  develope  the  first  order  Taylor  expansion  for  f^  and  f^ 
about  some  nominal  values  for  Ax^,  Ax^,  Ay^,  Aysl,  RQ,  <J>Q ,  (fi^,  <f>2, 
named  Ax^,  Ax^,  Ays(),  Aygl,  RQ,  <j>0,  4>1  and  <j>  .  Then  we  will  get  the 
following  equations: 


3f  3f  3f  3f  3f 

6  v  =  ttt - £Ax  +  -57 -  6 Ax  +  ttt —  5Ay  +  rr —  6Ay  +  •=— —  6Rn 

*  3Axs0  S°  3Axsl  Sl  3Ays0  S°  3Aysl  Sl  3R0  ° 


3f  3f,  3f 


(H6) 


3f. 


3f. 


3f, 


3f, 


3f, 


6v  =  —  5Ax  _  +  tt:--  -  6 Ax  .  +  37-^ —  6Ay  n  +  -57^ —  6Ay  +  -5-^  6r 

y  3AxsQ  sO  3Axsl  sx  3Ays()  2s0  3Aysl  Jsl  3rq  0 


3f 


3f 


3f„ 


+  3<j>-  **o  +  3(f),  5<|,1  +  3^  ~T2 


(H7) 


where : 


<5v  =  v  -  v  =  v  -  f.  (•) 

X  X  X  X  1 

(H8) 

6v  =  v  -  v  =  v  -  f  „  ( • ) 
y  y  y  y  2 

(H9) 
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6 Ax  _  =  Ax  _  —  Ax  - 
sO  sO  sO 


(HIO) 


<5 Ax  ,  =  Ax  ,  -  Ax  . 

si  si  Si 


“*80  "  4*S0-‘*S0 


“ysi  ■  Aysr4ysi 


{R0  -  VR0 


s*o  ■  V+o 


“i  -  V+i 


«*2  *  ♦2-*2 


The  velocity  components  of  the  covariance  matrix  are  now  given  by: 


(H12) 


(H13) 


(H15) 


{HI  6) 


(HI?) 


P33  P34 


P43  P44 


E{ (6v  )  }  e{5v  5v  } 
x  x  y 


e{6v  6v  }  E  ($v  )  } 
y  x  y 


We  now  assiane  that  Ax^,  Axgl,  Ay s(,,  Ay&1,  R0,  <j>0*  and  $2  ali  32:6 


statistical  independent.  Then  we  have 


(%)’•%« 
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p44  =  E{ (6vy)  } 


.  f  +  (!^_  )2  +  (!k_)2  +  (!M21 

LUxs0;  +\3A *al/  +UAys0/  +Ws/J 


a2  + 
s 


(H20) 


P34  P43 


r8fi 

=  H<Sv  Sv  }  =  -r-* -  • 

x  y  L3Axn 


3f„  3f ,  3f 
0  1  2 


A  3Ax  1  3Ax  3 Ax 

sO  sO  si  si 


3f, 


3f2  +  8f. 


3f- 


3AysO  3AysO  3Aysl  3Aysl 


] 


a2  + 

s 


3f 

3r, 


i  !fi  2  [’'i  at2 ,  3fi  “Tl, 

o  '  9*0  \  ^  S*o  9*j.  9+  +s*2  W2  J 


(H21) 


The  different  derivatives  of  f  and  f  are  given  in  the  following: 

JL 


3f 

3Ax 


1  1  T  Sin  ^2  COS  ^1  At2  | 

*To  =  V*0  I  +  Sin(<j,2‘<|,l)  * 


(H22) 


3fx  T  sin  <j>2  cos  <J>  1 

3AV  =  L1 "  sin(V^  -I 


(H23) 


3f, 


3Ay 


sO 


tTt 


sin  (Ji  At 

- "  )  *  Sin  *i  ^ 


2-u0  sin(*2-^1 


(H24) 


3f, 


1  Sln  *2 


3Aysl  =  t2_t0  Sin(<*,2-<f>l)  ’  Sin  4>1 


(H25) 


3f„ 


1  cos  $2  cos  4^  At2 


3AxsO  t2"t0  ®in(W  Ati 


(H26) 
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9f2 

1 

cos  <\>2  cos  <j>^ 

9AXsl 

V‘o 

sin($  -i^) 

9f  2 

-  1  Cl 

cos  4>2  *  sin  4^ 

9Ays0 

t2"t0  L 

sin($  -$  ) 

3f2 

-  1  li 

cos  <j>2  sin  4>^"| 

9Aysl 

L 

+  sin  (<4>2-4>1)  J 

9fl 

i  r4t2 

sin(4>1-<j)0)sin  (j>2 

3R0 

VtCl-Atl 

sin($2-$  ) 

9f  2 

i  r4t2 

sin  cos  <j>2 

9Ro" 

t2~to  *-Ati 

sin  (4*2^) 

-  sin  <f> 


-  cos  <p 


J 

J 


(H27) 


(H28) 


(H29) 


(H30) 


(H31) 


9fi  Ro  r  At2  cos  Sin  <P2 


*0  "  t2~t0  ‘  L  At 


sin  (4>2-4>i) 


+  COS  <j) 


J 


(H32) 


3f. 


34>f 


to-tn  L 


At2  cos  (<{>1-(J)0)  cos  <p2 


'2  “0  »-Atl 


sin  (4>2~^>x) 


-  sin  <j> 


J 


(H33) 


9fl 

3* 


» — i— .  r 

V'o  L 


At2  sin  <t>2  •  sin(<J)2-(ji0) 


sin  4>„ 


v0  At, 


2  0 
At  2 

SC"  Ms0  -  **31  COS  *2  + 


sin  (<j>2“<l>1) 


sin  (4>2-<l>1 


-  | 


Jcos  *2  Ays0-iysl]  »in  *2}j 


(H34) 


3f2 

3<f*n 


v,0  lro  itl  • 


cos  <j>2*sin(<j>2-<{>0) 
2 

sin  (4>2"<f>1) 


cos  <j). 


2 

sin  (4>3-<f>1) 


{ 


Ax  _  -  Ax  ,  cos  6*  + 
sO  si  r2 


jcos  t2  +[^Ays0  -  Ayjsinf,, 


(H35) 


